void RigidBody::step(double dt){
FILE_LOG(logDEBUG)<<"entered RigidBody::step(double dt)"<<std::endl;
// set com
// NOTE: com is transform r in THIS case
for(unsigned i=0;i<3;i++)
com[i] = Tws(i,3);
// NOTE: set accel for integration function
vd_ = &vd;
FILE_LOG(logDEBUG) << "q (before) = " << q << std::endl;
FILE_LOG(logDEBUG) << "v (before) = " << v << std::endl;
FILE_LOG(logDEBUG) << "vd (before) = " << vd << std::endl;
// Set forces to apply
// apply drag force
{
double k = 0.1;
Ravelin::Vector3d drag;
v.get_sub_vec(0,3,drag);
drag = -drag *= (drag.dot(drag) * k);
apply_force(drag,com);
}
// apply forces
calc_fwd_dynamics();
// Set up state
Vec state;
state.set_zero(q.rows() + v.rows());
state.set_sub_vec(0,q);
state.set_sub_vec(q.rows(),v);
// Integrate state forward 1 step
sim_->integrate(sim_->time,dt,&fn,state);
// re-normalize quaternion state
state.get_sub_vec(0,q.rows(),q);
state.get_sub_vec(q.rows(),state.rows(),v);
Vec e;
q.get_sub_vec(3,7,e);
e.normalize();
q.set_sub_vec(3,e);
FILE_LOG(logDEBUG) << "q (after) = " << q << std::endl;
FILE_LOG(logDEBUG) << "v (after) = " << v << std::endl;
FILE_LOG(logDEBUG) << "vd (after) = " << vd << std::endl;
// Update graphics transform
update();
FILE_LOG(logDEBUG)<<"exited RigidBody::step(.)"<<std::endl;
reset_accumulators();
}
void loop(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
const unsigned X=0,Y=1,Z=2,THETA=5;
// World frame
boost::shared_ptr<Ravelin::Pose3d>
environment_frame(new Ravelin::Pose3d());
Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Pose(*environment_frame.get())));
boost::shared_ptr<Ravelin::Pose3d>
base_horizontal_frame(new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_horizontal_frame")));
Ravelin::Vector3d com(base_horizontal_frame->x.data());
OUT_LOG(logERROR) << "x = " << base_horizontal_frame->x ;
Ravelin::VectorNd base_xd;
ctrl->get_base_value(Pacer::Robot::velocity,base_xd);
OUT_LOG(logERROR) << "xd = " << base_xd ;
/////////////////////
/// SET VARIABLES ///
// Command robot to walk in a direction
Ravelin::VectorNd command;
command.set_zero(6);
double max_forward_speed = ctrl->get_data<double>(plugin_namespace+".max-forward-speed");
double max_strafe_speed = ctrl->get_data<double>(plugin_namespace+".max-strafe-speed");
double max_turn_speed = ctrl->get_data<double>(plugin_namespace+".max-turn-speed");
// if FALSE, drive like a car (x and theta)
// Q: if TRUE, turn toward waypoint while stepping in direction of waypoint
bool HOLONOMIC = false;
/////////////////////////////////////
/// Assign WAYPOINTS in the plane ///
static std::vector<Point> waypoints;
std::vector<double> waypoints_vec = ctrl->get_data<std::vector<double> >(plugin_namespace+".waypoints");
// if (waypoints not inititalized) OR (waypoints changed) OR (only one waypoint)
// update waypoints
if(waypoints.empty() || waypoints_vec.size()/2 != waypoints.size() || waypoints_vec.size() == 2){
waypoints.clear();
for(int i=0;i<waypoints_vec.size();i+=2)
waypoints.push_back(Point(waypoints_vec[i],waypoints_vec[i+1]));
}
/////////////////////////////
/// CHOOSE NEXT WAYPOINT ///
int num_waypoints = waypoints.size();
// if waypoints are empty
// do not do anything (do not update SE2 command)
if(num_waypoints != 0){
Ravelin::Vector3d goto_point;
static int waypoint_index = 0;
static Ravelin::Vector3d
next_waypoint(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
if(waypoints.size() == 1){
waypoint_index = 0;
next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
}
double distance_to_wp = (Ravelin::Origin3d(next_waypoint.data()) - Ravelin::Origin3d(com[X],com[Y],0)).norm();
double max_dist = ctrl->get_data<double>(plugin_namespace+".tolerance");
if( distance_to_wp < max_dist){
OUT_LOG(logDEBUG1) << "waypoint reached, incrementing waypoint.";
OUTLOG(next_waypoint,"this_wp",logDEBUG1);
OUT_LOG(logDEBUG1) << "this waypoint: " << next_waypoint;
OUT_LOG(logDEBUG1) << "robot position: " << com;
waypoint_index = (waypoint_index+1)% num_waypoints;
if(waypoint_index == 0)
exit(0);
next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
}
OUT_LOG(logDEBUG1) << "num_wps = " << num_waypoints;
OUT_LOG(logDEBUG1) << "distance_to_wp = " << distance_to_wp;
OUT_LOG(logDEBUG1) << "waypoint_index = " << waypoint_index;
Utility::visualize.push_back(Pacer::VisualizablePtr(new Pacer::Ray(next_waypoint,com,Ravelin::Vector3d(1,0.5,0))));
OUT_LOG(logDEBUG1) << "next_wp" << next_waypoint;
for(int i=0;i<num_waypoints;i++){
Ravelin::Vector3d wp(waypoints[i].first,waypoints[i].second,0,environment_frame);
OUT_LOG(logDEBUG1) << "\twp" << wp;
Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Point(wp,Ravelin::Vector3d(1,0.5,0),0.1)));
}
goto_point = next_waypoint;
///////////////////////
/// GO TO WAYPOINT ///
// Find direction to waypoint from robot position
Ravelin::Vector3d goto_direction =
Ravelin::Vector3d(goto_point[X],goto_point[Y],0,environment_frame)
- Ravelin::Vector3d(com[X],com[Y],0,environment_frame);
goto_direction = Ravelin::Pose3d::transform_vector(base_horizontal_frame,goto_direction);
goto_direction.normalize();
double angle_to_goal = atan2(goto_direction[Y],goto_direction[X]);
OUT_LOG(logDEBUG) << "angle_to_goal = " << angle_to_goal;
// If robot is facing toward goal already, walk in that direction
if(fabs(angle_to_goal) < M_PI_4){
if(HOLONOMIC){
command[Y] = goto_direction[Y]*max_strafe_speed;
}
command[X] = goto_direction[X]*max_forward_speed;
command[THETA] = angle_to_goal;
} else {
command[THETA] = Utility::sign(angle_to_goal)*max_turn_speed;
if(HOLONOMIC){
command[X] = goto_direction[X]*max_forward_speed;
command[Y] = goto_direction[Y]*max_strafe_speed;
} else {
command[X] = 0;
command[Y] = 0;
}
}
// double slow_down_tol = 0.1;
// if(distance_to_wp < slow_down_tol)
// command *= distance_to_wp*slow_down_tol+0.05;
Ravelin::Origin3d command_SE2(command[X],command[Y],command[THETA]);
ctrl->set_data<Ravelin::Origin3d>("SE2_command",command_SE2);
}
}
Ravelin::Vector3d Utility::slerp( const Ravelin::Vector3d& v0,const Ravelin::Vector3d& v1,double t){
double a = v0.dot(v1);
return (v0*sin((1-t)*a)/sin(a) + v1*sin(t*a)/sin(a));
}
double Utility::distance_from_plane(const Ravelin::Vector3d& normal,const Ravelin::Vector3d& point, const Ravelin::Vector3d& x){
Ravelin::Vector3d v;
v = x - point;
return normal.dot(v);
}
/// Gets the distance from a cylinder primitive
double PlanePrimitive::calc_signed_dist(shared_ptr<const CylinderPrimitive> pA, Point3d& pthis, Point3d& pcyl) const
{
FILE_LOG(LOG_COLDET) << "PlanePrimitive::calc_signed_dist(.) entered" << std::endl;
// Output Params
// Set intitial value of distance to contact
double d = std::numeric_limits<double>::infinity();
// get the pose for the plane primitive
boost::shared_ptr<const Ravelin::Pose3d> Pplane = pthis.pose;
// get the pose for the cylinder
boost::shared_ptr<const Ravelin::Pose3d> Pcyl = pcyl.pose;
const double R = pA->get_radius();
const double H = pA->get_height();
const unsigned X = 0, Y = 1,Z = 2;
// Cylinder to Plane frame Tranformation
Ravelin::Transform3d pPc = Ravelin::Pose3d::calc_relative_pose(Pcyl,Pplane);
// Cylinder axis (in plane frame) cN
// Take Y column from Cylinder to Plane frame Tranformation
Ravelin::Vector3d cN = Ravelin::Vector3d(
Ravelin::Matrix3d(pPc.q).get_column(Y),
Pplane);
cN.normalize();
// Plane Normal is Y axis in local frame
Ravelin::Vector3d n(0,1,0,Pplane);
// cylinder origin w.r.t. plane
Point3d c0(pPc.x.data(),Pplane);
double n_dot_cN = n.dot(cN);
// Axial direction points toward plane parallel to axis of cylinder
Ravelin::Vector3d axial_dir = cN;
if(n_dot_cN > 0)
axial_dir = -axial_dir;
axial_dir.normalize();
// if Cylinder is aligned with plane:
// Return distance cylinder origin to
// closest point on plane minus height
if(fabs(n_dot_cN) > 1.0-1e-8){
FILE_LOG(LOG_COLDET) << " -- Cylinder axis is perpendicular to Plane" << std::endl;
// Measure from center of closest face to the plane
Point3d x = (H/2.0)*axial_dir + c0;
// Distance above plane
d = x.dot(n);
// Find point on plane that x is closest to
pcyl = Ravelin::Pose3d::transform_point(Moby::GLOBAL,x);
Point3d pP = x - d*n;
pthis = Ravelin::Pose3d::transform_point(Moby::GLOBAL,pP);
} else if(fabs(n_dot_cN) < 1e-8){
FILE_LOG(LOG_COLDET) << " -- Cylinder axis is parallel to Plane"<< std::endl;
// Measure from center of closest edge (on curved face) to the plane
// Radial direction and Plane normal coincide in this case
Point3d x = c0 - R*n + (H/2.0)*axial_dir;
// Distance above plane
d = x.dot(n);
pcyl = Ravelin::Pose3d::transform_point(Moby::GLOBAL,x);
Point3d pP = x - d*n;
pthis = Ravelin::Pose3d::transform_point(Moby::GLOBAL,pP);
}
else {
FILE_LOG(LOG_COLDET) << " -- Cylinder edge is closest to plane"<< std::endl;
//(axis_cylinder x (n_plane x axis_cylinder))
// Radial direction points toward plane perpendicular to axis of cylinder
Ravelin::Vector3d radial_dir =
Ravelin::Vector3d::cross(
axial_dir,
Ravelin::Vector3d::cross(axial_dir,n)
);
radial_dir.normalize();
// (--(|)
// Through axis, toward plane "axial direction"^, ^on face, toward plane "radial direction"
FILE_LOG(LOG_COLDET) << "axial_dir = " << axial_dir << std::endl;
FILE_LOG(LOG_COLDET) << "p_axis = " << n << std::endl;
FILE_LOG(LOG_COLDET) << "radial_dir = " << radial_dir << std::endl;
// Measure from point to the plane
Point3d x = c0 + (H/2.0)*axial_dir + R*radial_dir;
FILE_LOG(LOG_COLDET) << "x_cyl (plane frame) = " << x << std::endl;
// Distance above plane
d = x.dot(n);
Point3d pP = x - d*n;
FILE_LOG(LOG_COLDET) << "x_plane (plane frame) = " << x << std::endl;
pcyl = Ravelin::Pose3d::transform_point(Moby::GLOBAL,x);
pthis = Ravelin::Pose3d::transform_point(Moby::GLOBAL,pP);
}
FILE_LOG(LOG_COLDET) << "Point cylinder (global frame): "<< pcyl << std::endl;
FILE_LOG(LOG_COLDET) << "Point plane (global frame): "<< pthis << std::endl;
FILE_LOG(LOG_COLDET) << "distance: "<< d << std::endl;
return d;
}