bool auto_move::calculate_effect(HIP& hip, double* out_force) { // 1cm/sec at a depth of 1cm. == 1m/sec at 1m const static double velocity_scale = 1; // units 1 (cm/sec)/cm = 1/sec bool contact = false; btVector3 pos_incr; for (int i=0;i<3;i++) out_force[i]=0; // Calculate desired velocity // Velocity command is a function of penetration depth into the feature btVector3 velocity = normalized_direction_to_target(hip) * velocity_scale * penetration_depth(hip); // units: m/sec // Calculate a position increment1 // pos_incr = timestep * vel_vector = t * d/t if (hip.get_last_time_incr() < TENTH_SECOND) { pos_incr = hip.get_last_time_incr() * velocity; } // Send position increment to robot publish_automove(pos_incr, hip.id()); return contact; }
/*** * * Assistive features * jestream--- auto move with velocity proportional to height * * ***/ bool jetstream::calculate_effect(HIP& hip, double* out_force) { // 1 = 1cm/sec at a depth of 1cm. == 1m/sec at 1m const static double velocity_scale = 2; // units 1 (cm/sec)/cm = 1/sec btVector3 pos_incr; for (int i=0;i<3;i++) out_force[i]=0; bool contact = is_in_contact(hip); if ( contact && distance_to_target(hip) > target_radius) { // Calculate desired velocity // Velocity command is a function of penetration depth into the feature btVector3 velocity = normalized_direction_to_target(hip) * velocity_scale * sqrt(penetration_depth(hip)); // units: m/sec // Calculate a position increment1 if (hip.get_last_time_incr() < TENTH_SECOND) { pos_incr = hip.get_last_time_incr() * velocity; } // Send position increment to robot publish_automove(pos_incr, hip.id()); } return contact; }
bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Convex& b, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { DT_HybridPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b, b_margin); MT_Scalar max_pen_len = MT_Scalar(0.0); return penetration_depth(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, v, pa, pb, max_pen_len); }
bool jetstream::is_in_contact(HIP& hip) { return ( penetration_depth(hip) != 0 ); }
DT_Bool DT_Encounter::exactTest(const DT_RespTable *respTable, int& count) const { const DT_ResponseList& responseList = respTable->find(m_obj_ptr1, m_obj_ptr2); switch (responseList.getType()) { case DT_SIMPLE_RESPONSE: if (intersect(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis)) { ++count; return (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) ? responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), 0) : responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), 0); } break; case DT_WITNESSED_RESPONSE: { MT_Point3 p1, p2; if (common_point(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis, p1, p2)) { ++count; if (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) { DT_CollData coll_data; p1.getValue(coll_data.point1); p2.getValue(coll_data.point2); return responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), &coll_data); } else { DT_CollData coll_data; p1.getValue(coll_data.point2); p2.getValue(coll_data.point1); return responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), &coll_data); } } break; } case DT_DEPTH_RESPONSE: { MT_Point3 p1, p2; if (penetration_depth(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis, p1, p2)) { ++count; if (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) { DT_CollData coll_data; p1.getValue(coll_data.point1); p2.getValue(coll_data.point2); (p2 - p1).getValue(coll_data.normal); return responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), &coll_data); } else { DT_CollData coll_data; p1.getValue(coll_data.point2); p2.getValue(coll_data.point1); (p1 - p2).getValue(coll_data.normal); return responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), &coll_data); } } break; } case DT_NO_RESPONSE: break; default: assert(false); } return DT_CONTINUE; }