Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2492] pkg/trunk
|
From: <hsu...@us...> - 2008-08-02 22:10:25
|
Revision: 2492
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2492&view=rev
Author: hsujohnhsu
Date: 2008-08-02 22:10:25 +0000 (Sat, 02 Aug 2008)
Log Message:
-----------
updates to use new grippers.
Modified Paths:
--------------
pkg/trunk/clean_rosgazebo.scp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/clean_rosgazebo.scp
===================================================================
--- pkg/trunk/clean_rosgazebo.scp 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/clean_rosgazebo.scp 2008-08-02 22:10:25 UTC (rev 2492)
@@ -1,7 +1,8 @@
#!/bin/bash
#clean controllers
-(cd controllers/genericControllers ; rm -f lib/* ;make clean)
+(cd controllers/generic_controllers ; rm -f lib/* ;make clean)
+(cd deprecated/genericControllers ; rm -f lib/* ;make clean)
(cd controllers/pr2Controllers ; rm -f lib/* ;make clean)
#clean gazebo dependent stuff
@@ -13,7 +14,7 @@
(cd drivers/simulator/gazebo_sensors ; rm -f lib/* ;make clean)
(cd simulators/rosgazebo ; rm -f lib/* ;make clean)
-(cd robot/pr2_gazebo ; rm -f lib/* ;make clean)
+(cd robot_control_loops/pr2_gazebo ; rm -f lib/* ;make clean)
#clean 2dnav stack stuff
(cd nav/nav_view ; rm -f lib/* ;make clean)
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-08-02 22:10:25 UTC (rev 2492)
@@ -66,6 +66,7 @@
enum PR2_JOINT_TYPE{
PRISMATIC, ROTARY,
+ GRIPPER,
ROTARY_CONTINUOUS,
MAX_JOINT_TYPES
};
@@ -91,6 +92,7 @@
ARM_L_ELBOW_ROLL ,
ARM_L_WRIST_PITCH ,
ARM_L_WRIST_ROLL ,
+ ARM_L_GRIPPER_GAP , // added 20080802 by john
ARM_R_PAN ,
ARM_R_SHOULDER_PITCH,
ARM_R_SHOULDER_ROLL,
@@ -98,6 +100,7 @@
ARM_R_ELBOW_ROLL ,
ARM_R_WRIST_PITCH ,
ARM_R_WRIST_ROLL ,
+ ARM_R_GRIPPER_GAP , // added 20080802 by john
HEAD_YAW ,
HEAD_PITCH ,
HEAD_LASER_PITCH ,
@@ -298,7 +301,7 @@
// JointStart/JointEnd corresponds to the PR2_MODEL_ID, start and end id for each model
enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ , MAX_MODELS };
const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
- const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
+ const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_GRIPPER_GAP,ARM_R_GRIPPER_GAP,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
// Geometric description for the base
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-08-02 22:10:25 UTC (rev 2492)
@@ -102,6 +102,30 @@
// explicit damping coefficient for the joint
private: double dampCoef;
+ // get joints names from xml fields
+ private: std::string finger_l_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: std::string finger_tip_l_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: std::string finger_r_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: std::string finger_tip_r_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // get the joints from parent
+ private: HingeJoint *finger_l_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: HingeJoint *finger_tip_l_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: HingeJoint *finger_r_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: HingeJoint *finger_tip_r_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // assign pid for each finger for PD_CONTROL
+ private: controller::Pid *finger_l_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: controller::Pid *finger_tip_l_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: controller::Pid *finger_r_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: controller::Pid *finger_tip_r_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // get name of each child, e.g. front_left_caster_steer
+ std::string actarrayName[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // get type of each child, only check for special case for grippers for now. All others ignored. (TODO)
+ std::string actarrayType[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc 2008-08-02 22:10:25 UTC (rev 2492)
@@ -115,80 +115,120 @@
SliderJoint *sjoint;
HingeJoint *hjoint;
-
// get children of the actarray, add to actuators object list
- int i =0;
- for (i=0, jNode = node->GetChild("joint"); jNode; i++)
+ int count =0;
+ for (jNode = node->GetChild("joint"); jNode ;)
{
// add each child
//actuators.AddJoint();
// get name of each child, e.g. front_left_caster_steer
- std::string name = jNode->GetString("name","",1);
+ actarrayName[count] = jNode->GetString("name","",1);
- // dynamic cast for type checking
- Joint* tmpJoint = dynamic_cast<Joint*>(this->myParent->GetJoint(name));
+ // get type of each child, only check for special case for grippers for now. All others ignored. (TODO)
+ actarrayType[count] = jNode->GetString("type","default",0); // FIXME: not mandatory for now
- // check what type of joint this is
- switch(tmpJoint->GetType())
+ if (actarrayType[count]=="gripper_special") // if this is a gripper, do something about it
{
- case Joint::SLIDER:
- // save joint in list
- this->joints[i] = tmpJoint;
- // set joint properties
- sjoint = dynamic_cast<SliderJoint*>( tmpJoint );
- // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
- this->myIface->data->actuators[i].actualPosition = sjoint->GetPosition();
- this->myIface->data->actuators[i].actualSpeed = sjoint->GetPositionRate();
- this->myIface->data->actuators[i].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- this->myIface->data->actuators[i].jointType = PR2::PRISMATIC;
- break;
- case Joint::HINGE:
- // save joint in list
- this->joints[i] = tmpJoint;
- // set joint properties
- hjoint = dynamic_cast<HingeJoint*>( tmpJoint );
- // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
- this->myIface->data->actuators[i].actualPosition = hjoint->GetAngle();
- this->myIface->data->actuators[i].actualSpeed = hjoint->GetAngleRate();
- this->myIface->data->actuators[i].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- this->myIface->data->actuators[i].jointType = PR2::ROTARY;
- break;
- case Joint::HINGE2:
- case Joint::BALL:
- case Joint::UNIVERSAL:
- gzthrow("Pr2_Actarray joint need to be either slider or hinge for now. Add support in Pr2_Actarray or talk to ");
- break;
+ std::cout << " gripper_special joint type " << actarrayType[count] << " " << jNode->GetString("type","default",0) << std::endl;
+ // get joints names from xml fields
+ finger_l_name [count] = jNode->GetString("left_proximal","",1);
+ finger_tip_l_name [count] = jNode->GetString("left_distal","",1);
+ finger_r_name [count] = jNode->GetString("right_proximal","",1);
+ finger_tip_r_name [count] = jNode->GetString("right_distal","",1);
+
+ // get the joints from parent, store the pointers to the joints locally in this class
+ finger_l_joint [count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_l_name [count]));
+ finger_tip_l_joint[count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_tip_l_name[count]));
+ finger_r_joint [count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_r_name [count]));
+ finger_tip_r_joint[count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_tip_r_name[count]));
+
+ // all four joints controlled by a single iface settings value
+ // IMPORTANT: in Iface, we'll use position as the gap command
+ // IMPORTANT: in Iface, if in torque control mode, pass through torque to all joints with the right sign
+ // given a single gap command, we can find out what the joint angles are
+ // FIXME: assuming for now that all joints are hinges
+
+ // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
+ //this->myIface->data->actuators[count].actualPosition = finger_l_joint[count]->GetAngle(); //TODO: get gap from joint positions
+ //this->myIface->data->actuators[count].actualSpeed = finger_l_joint[count]->GetAngleRate(); //TODO: get gap rate?
+ //this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ this->myIface->data->actuators[count].jointType = PR2::GRIPPER;
+
+ // create new pid for each finger for PD_CONTROL
+ this->finger_l_pids [count] = new Pid();
+ this->finger_r_pids [count] = new Pid();
+ this->finger_tip_l_pids[count] = new Pid();
+ this->finger_tip_r_pids[count] = new Pid();
}
+ else // if this is not a gripper, process them as sliders and hinges
+ {
+ std::cout << " default joint type " << actarrayType[count] << " " << jNode->GetString("type","default",0) << std::endl;
+ // dynamic cast for type checking
+ Joint* tmpJoint = dynamic_cast<Joint*>(this->myParent->GetJoint(actarrayName[count]));
- this->myIface->data->actuators[i].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
- this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain" ,0.0,1);
- this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain" ,0.0,1);
- this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain" ,0.0,1);
- this->myIface->data->actuators[i].iClamp = jNode->GetDouble("iClamp",0.0,1);
- //this->myIface->data->actuators[i].cmdPosition = 0.0;
- //this->myIface->data->actuators[i].cmdSpeed = 0.0;
- std::string tmpControlMode = jNode->GetString("controlMode","PD_CONTROL",0);
- this->myIface->data->actuators[i].dampingCoefficient = jNode->GetDouble("explicitDampingCoefficient",0.0,0);
+ // check what type of joint this is
+ switch(tmpJoint->GetType())
+ {
+ case Joint::SLIDER:
+ // save joint in list
+ this->joints[count] = tmpJoint;
+ // set joint properties
+ sjoint = dynamic_cast<SliderJoint*>( tmpJoint );
+ // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
+ this->myIface->data->actuators[count].actualPosition = sjoint->GetPosition();
+ this->myIface->data->actuators[count].actualSpeed = sjoint->GetPositionRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ this->myIface->data->actuators[count].jointType = PR2::PRISMATIC;
+ break;
+ case Joint::HINGE:
+ // save joint in list
+ this->joints[count] = tmpJoint;
+ // set joint properties
+ hjoint = dynamic_cast<HingeJoint*>( tmpJoint );
+ // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
+ this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
+ this->myIface->data->actuators[count].actualSpeed = hjoint->GetAngleRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ this->myIface->data->actuators[count].jointType = PR2::ROTARY;
+ break;
+ case Joint::HINGE2:
+ case Joint::BALL:
+ case Joint::UNIVERSAL:
+ gzthrow("Pr2_Actarray joint need to be either slider or hinge for now. Add support in Pr2_Actarray or talk to ");
+ break;
+ }
+ }
+
+ // get additional information about this actuator
+ this->myIface->data->actuators[count].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
+ this->myIface->data->actuators[count].pGain = jNode->GetDouble("pGain" ,0.0,1);
+ this->myIface->data->actuators[count].iGain = jNode->GetDouble("iGain" ,0.0,1);
+ this->myIface->data->actuators[count].dGain = jNode->GetDouble("dGain" ,0.0,1);
+ this->myIface->data->actuators[count].iClamp = jNode->GetDouble("iClamp",0.0,1);
+ std::string tmpControlMode = jNode->GetString("controlMode","PD_CONTROL",0);
+ this->myIface->data->actuators[count].dampingCoefficient = jNode->GetDouble("explicitDampingCoefficient",0.0,0);
+
// init a new pid for this joint
- this->pids[i] = new Pid();
+ this->pids[count] = new Pid();
// get time
- this->myIface->data->actuators[i].timestamp = Simulator::Instance()->GetSimTime();
+ this->myIface->data->actuators[count].timestamp = Simulator::Instance()->GetSimTime();
// set default control mode to:
if (tmpControlMode == "TORQUE_CONTROL")
- this->myIface->data->actuators[i].controlMode = PR2::TORQUE_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::TORQUE_CONTROL;
else if (tmpControlMode == "PD_TORQUE_CONTROL")
- this->myIface->data->actuators[i].controlMode = PR2::PD_TORQUE_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::PD_TORQUE_CONTROL;
else if (tmpControlMode == "PD_CONTROL")
- this->myIface->data->actuators[i].controlMode = PR2::PD_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::PD_CONTROL;
else
- this->myIface->data->actuators[i].controlMode = PR2::PD_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::PD_CONTROL;
jNode = jNode->GetNext("joint");
+ count++;
}
- this->num_joints = i;
+ this->num_joints = count;
}
////////////////////////////////////////////////////////////////////////////////
@@ -202,19 +242,39 @@
{
for (int count = 0; count < this->num_joints ; count++)
{
- // initialize pid stuff
- this->pids[count]->InitPid( this->myIface->data->actuators[count].pGain,
- this->myIface->data->actuators[count].iGain,
- this->myIface->data->actuators[count].dGain,
- this->myIface->data->actuators[count].iClamp,
- -this->myIface->data->actuators[count].iClamp
- );
- this->pids[count]->SetCurrentCmd(0);
- // as a first hack, initialize to zero velocity and saturation torque
- //this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
- //this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- this->joints[count]->SetParam( dParamVel , 0);
- this->joints[count]->SetParam( dParamFMax, 0);
+ if (actarrayType[count]=="gripper_special") // if this is a gripper, do something about it
+ {
+ // FIXME: temporary hack, initialize finger pid stuff, this should be based on transmissions
+ this->finger_l_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_r_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_tip_l_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_tip_r_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_l_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_r_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_tip_l_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_tip_r_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_l_joint[count] -> SetParam( dParamFMax, 0 );
+ this->finger_r_joint[count] -> SetParam( dParamFMax, 0 );
+ this->finger_tip_l_joint[count] -> SetParam( dParamFMax, 0 );
+ this->finger_tip_r_joint[count] -> SetParam( dParamFMax, 0 );
+ }
+ else
+ {
+ // initialize pid stuff
+ this->pids[count]->InitPid( this->myIface->data->actuators[count].pGain,
+ this->myIface->data->actuators[count].iGain,
+ this->myIface->data->actuators[count].dGain,
+ this->myIface->data->actuators[count].iClamp,
+ -this->myIface->data->actuators[count].iClamp
+ );
+ this->pids[count]->SetCurrentCmd(0);
+
+ // as a first hack, initialize to zero velocity and saturation torque
+ //this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
+ //this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ this->joints[count]->SetParam( dParamVel , 0);
+ this->joints[count]->SetParam( dParamFMax, 0);
+ }
}
lastTime = Simulator::Instance()->GetSimTime();
}
@@ -350,158 +410,280 @@
//////////////////////////////////////////////////////////////////////
for (int count = 0; count < this->num_joints; count++)
{
- switch(this->joints[count]->GetType())
+ if (actarrayType[count]=="gripper_special") // if this is a gripper, do something about it
{
- case Joint::SLIDER:
- sjoint = dynamic_cast<SliderJoint*>(this->joints[count]);
- cmdPosition = this->myIface->data->actuators[count].cmdPosition;
- cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
+ // get commands
+ cmdPosition = this->myIface->data->actuators[count].cmdPosition;
+ cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
+ // translate command position into joint angle
+ // command position is in radians
+ static double angleGapRatio = 0.2 / M_PI * 4.0;
+ double cmdGripperJointAngle = cmdPosition / angleGapRatio; // FIXME: TEMP HACK for transmission, 45 degrees with gripper gap command of 0.2 (meters)
- switch(this->myIface->data->actuators[count].controlMode)
- {
- case PR2::TORQUE_CONTROL :
- sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
- break;
- // case PR2::PD_CONTROL1 :
- // // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- // positionError = cmdPosition - sjoint->GetPosition();
- // speedError = cmdSpeed - sjoint->GetPositionRate();
- // //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- // currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- // currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // sjoint->SetSliderForce(currentCmd);
- // break;
- case PR2::PD_TORQUE_CONTROL :
- case PR2::PD_CONTROL : // velocity control
- //if (cmdPosition > sjoint->GetHighStop())
- // cmdPosition = sjoint->GetHighStop();
- //else if (cmdPosition < sjoint->GetLowStop())
- // cmdPosition = sjoint->GetLowStop();
-
- positionError = sjoint->GetPosition() - cmdPosition; //Error defined as actual - desired
- speedError = sjoint->GetPositionRate() - cmdSpeed;
- //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
-
- sjoint->SetParam( dParamVel , currentCmd );
- sjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- break;
- case PR2::SPEED_CONTROL :
- sjoint->SetParam( dParamVel, cmdSpeed);
- sjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- break;
-
- default:
- break;
- }
+ // get control mode
+ switch(this->myIface->data->actuators[count].controlMode)
+ {
+ case PR2::TORQUE_CONTROL:
+ // EXPERIMENTAL: add explicit damping
+ /* ----------- finger_l ---------- */
+ currentRate = finger_l_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ //printf("Damping f %f v %f\n",dampForce,currentRate);
+ //std::cout<<"Force is:"<< this->myIface->data->actuators[count].cmdEffectorForce + dampForce<<std::endl;
+ // simply set torque
+ finger_l_joint[count]->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
+ //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
+ /* ----------- finger_tip_l ---------- */
+ currentRate = finger_tip_l_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ finger_tip_l_joint[count]->SetTorque(-(this->myIface->data->actuators[count].cmdEffectorForce + dampForce)); // negative enforces parallel
+ /* ----------- finger_r ---------- */
+ currentRate = finger_r_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ finger_r_joint[count]->SetTorque(-(this->myIface->data->actuators[count].cmdEffectorForce + dampForce)); // negative enforces symmetry
+ /* ----------- finger_tip_r ---------- */
+ currentRate = finger_tip_r_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ finger_tip_r_joint[count]->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
+ break;
+ case PR2::PD_TORQUE_CONTROL :
+ break;
+ case PR2::SPEED_TORQUE_CONTROL :
+ break;
+ case PR2::PD_CONTROL:
+ // positive angle = open gripper
+ // no negative angle
+ /* ----------- finger_l ---------- */
+ currentAngle = finger_l_joint[count]->GetAngle();
+ currentRate = finger_l_joint[count]->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle - cmdGripperJointAngle);
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->finger_l_pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ finger_l_joint[count]->SetParam( dParamVel, currentCmd );
+ finger_l_joint[count]->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- this->myIface->data->actuators[count].actualPosition = sjoint->GetPosition();
- this->myIface->data->actuators[count].actualSpeed = sjoint->GetPositionRate();
- this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- break;
+ /* ----------- finger_tip_l ---------- */
+ currentAngle = finger_tip_l_joint[count]->GetAngle();
+ currentRate = finger_tip_l_joint[count]->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle + cmdGripperJointAngle); // negative command angle enforces parallel gripper
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->finger_tip_l_pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ finger_tip_l_joint[count]->SetParam( dParamVel, currentCmd );
+ finger_tip_l_joint[count]->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- case Joint::HINGE:
- hjoint = dynamic_cast<HingeJoint*>(this->joints[count]);
- cmdPosition = this->myIface->data->actuators[count].cmdPosition;
- cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
- switch(this->myIface->data->actuators[count].controlMode)
- {
- case PR2::TORQUE_CONTROL:
- // TODO: EXPERIMENTAL: add explicit damping
- currentRate = hjoint->GetAngleRate();
- dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
- dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
- dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
- //printf("Damping f %f v %f\n",dampForce,currentRate);
- //std::cout<<"Force is:"<< this->myIface->data->actuators[count].cmdEffectorForce + dampForce<<std::endl;
- // simply set torque
- hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
- //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
- break;
- case PR2::PD_TORQUE_CONTROL :
- // TODO: EXPERIMENTAL: add explicit damping
- currentRate = hjoint->GetAngleRate();
- dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
- dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
- dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
- //printf("Damping f %f v %f\n",dampForce,currentRate);
+ /* ----------- finger_r ---------- */
+ currentAngle = finger_r_joint[count]->GetAngle();
+ currentRate = finger_r_joint[count]->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle + cmdGripperJointAngle); // negative command angle enforces symmetric gripper
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->finger_r_pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ finger_r_joint[count]->SetParam( dParamVel, currentCmd );
+ finger_r_joint[count]->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- currentAngle = hjoint->GetAngle();
- // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- positionError = ModNPi2Pi( currentAngle - cmdPosition);
- speedError = currentRate- cmdSpeed;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- // if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
- //Write out data
- if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << currentTime<<" "<<cmdPosition<<" "<<currentAngle<<" "<<positionError<< " "<<currentCmd << std::endl;
+ /* ----------- finger_tip_r ---------- */
+ currentAngle = finger_tip_r_joint[count]->GetAngle();
+ currentRate = finger_tip_r_joint[count]->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle - cmdGripperJointAngle);
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->finger_tip_r_pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ finger_tip_r_joint[count]->SetParam( dParamVel, currentCmd );
+ finger_tip_r_joint[count]->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- // limit torque
- currentCmd = (currentCmd > 100) ? 100: currentCmd;
- currentCmd = (currentCmd < -100 ) ? -100: currentCmd;
- //currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
- //currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
- hjoint->SetParam( dParamFMax, 0);
- hjoint->SetTorque(currentCmd + dampForce);
+ break;
+ case PR2::SPEED_CONTROL:
+ // positive speed = open speed, (since positive angle implies open gripper)
+ finger_l_joint[count]->SetParam( dParamVel , cmdSpeed);
+ finger_l_joint[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ finger_r_joint[count]->SetParam( dParamVel , -cmdSpeed);
+ finger_r_joint[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ finger_tip_l_joint[count]->SetParam( dParamVel , cmdSpeed);
+ finger_tip_l_joint[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ finger_tip_r_joint[count]->SetParam( dParamVel , -cmdSpeed);
+ finger_tip_r_joint[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ break;
+ case PR2::DISABLED:
+ finger_l_joint[count]->SetParam( dParamFMax, 0);
+ finger_l_joint[count]->SetTorque(0); //disable the joint
+ finger_r_joint[count]->SetParam( dParamFMax, 0);
+ finger_r_joint[count]->SetTorque(0); //disable the joint
+ finger_tip_l_joint[count]->SetParam( dParamFMax, 0);
+ finger_tip_l_joint[count]->SetTorque(0); //disable the joint
+ finger_tip_r_joint[count]->SetParam( dParamFMax, 0);
+ finger_tip_r_joint[count]->SetTorque(0); //disable the joint
+ break;
+ default:
+ break;
- break;
- case PR2::SPEED_TORQUE_CONTROL :
- currentRate = hjoint->GetAngleRate();
- speedError = currentRate - cmdSpeed;
- currentCmd = this->pids[count]->UpdatePid(speedError, currentTime-this->lastTime);
- //if(count==PR2::ARM_R_PAN)std::cout<<"Joint:" <<count << " Desired:" << cmdSpeed << " Current speed"<<currentRate<<" Error"<<speedError<<" cmd: " << currentCmd << std::endl;
- if(count==PR2::ARM_R_ELBOW_PITCH )std::cout<<currentTime<<" "<<currentRate<<" "<<speedError<<" " << currentCmd << std::endl;
- // limit torque
-
- currentCmd = (currentCmd > 100) ? 100: currentCmd;
- currentCmd = (currentCmd < -100 ) ? -100: currentCmd;
-
- //Needs to be set to 0 1x for every joint
- hjoint->SetParam( dParamFMax, 0);
- hjoint->SetTorque(currentCmd);
+ }
+ this->myIface->data->actuators[count].actualPosition = finger_l_joint[count]->GetAngle()*angleGapRatio; // FIXME: TEMP HACK for transmission, 45 degrees with gripper gap command of 0.2 (meters)
+ this->myIface->data->actuators[count].actualSpeed = finger_l_joint[count]->GetAngleRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+
+
+ }
+ else // if this is not a gripper, process them as sliders and hinges
+ {
+ switch(this->joints[count]->GetType())
+ {
+ case Joint::SLIDER:
+ sjoint = dynamic_cast<SliderJoint*>(this->joints[count]);
+ cmdPosition = this->myIface->data->actuators[count].cmdPosition;
+ cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
+
+ switch(this->myIface->data->actuators[count].controlMode)
+ {
+ case PR2::TORQUE_CONTROL :
+ sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
+ break;
+ // case PR2::PD_CONTROL1 :
+ // // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
+ // positionError = cmdPosition - sjoint->GetPosition();
+ // speedError = cmdSpeed - sjoint->GetPositionRate();
+ // //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
+ // currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ // currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ // currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ // sjoint->SetSliderForce(currentCmd);
+ // break;
+ case PR2::PD_TORQUE_CONTROL :
+ case PR2::PD_CONTROL : // velocity control
+ //if (cmdPosition > sjoint->GetHighStop())
+ // cmdPosition = sjoint->GetHighStop();
+ //else if (cmdPosition < sjoint->GetLowStop())
+ // cmdPosition = sjoint->GetLowStop();
+ positionError = sjoint->GetPosition() - cmdPosition; //Error defined as actual - desired
+ speedError = sjoint->GetPositionRate() - cmdSpeed;
+ //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+
+ sjoint->SetParam( dParamVel , currentCmd );
+ sjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ break;
+ case PR2::SPEED_CONTROL :
+ sjoint->SetParam( dParamVel, cmdSpeed);
+ sjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
+ break;
+
+ default:
+ break;
+ }
- break;
- case PR2::PD_CONTROL:
- //if (cmdPosition > hjoint->GetHighStop())
- // cmdPosition = hjoint->GetHighStop();
- //else if (cmdPosition < hjoint->GetLowStop())
- // cmdPosition = hjoint->GetLowStop();
-
- currentAngle = hjoint->GetAngle();
- currentRate = hjoint->GetAngleRate();
- positionError = ModNPi2Pi(currentAngle - cmdPosition);
- speedError = currentRate - cmdSpeed;
- //std::cout << "hinge e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- hjoint->SetParam( dParamVel, currentCmd );
- hjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- break;
- case PR2::SPEED_CONTROL:
- // printf("Hinge Speed Control\n");
- //std::cout << "wheel drive: " << cmdSpeed << " i " << count << std::endl;
- hjoint->SetParam( dParamVel, cmdSpeed);
- hjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- break;
- case PR2::DISABLED:
- hjoint->SetParam( dParamFMax, 0);
- hjoint->SetTorque(0); //disable the joint
- break;
- default:
- break;
+ this->myIface->data->actuators[count].actualPosition = sjoint->GetPosition();
+ this->myIface->data->actuators[count].actualSpeed = sjoint->GetPositionRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ break;
- }
- this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
- this->myIface->data->actuators[count].actualSpeed = hjoint->GetAngleRate();
- this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- break;
- case Joint::HINGE2:
- case Joint::BALL:
- case Joint::UNIVERSAL:
- break;
- }
- }
+ case Joint::HINGE:
+ hjoint = dynamic_cast<HingeJoint*>(this->joints[count]);
+ cmdPosition = this->myIface->data->actuators[count].cmdPosition;
+ cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
+ switch(this->myIface->data->actuators[count].controlMode)
+ {
+ case PR2::TORQUE_CONTROL:
+ // TODO: EXPERIMENTAL: add explicit damping
+ currentRate = hjoint->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ //printf("Damping f %f v %f\n",dampForce,currentRate);
+ //std::cout<<"Force is:"<< this->myIface->data->actuators[count].cmdEffectorForce + dampForce<<std::endl;
+ // simply set torque
+ hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
+ //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
+ break;
+ case PR2::PD_TORQUE_CONTROL :
+ // TODO: EXPERIMENTAL: add explicit damping
+ currentRate = hjoint->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ //printf("Damping f %f v %f\n",dampForce,currentRate);
+ currentAngle = hjoint->GetAngle();
+ // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
+ positionError = ModNPi2Pi( currentAngle - cmdPosition);
+ speedError = currentRate- cmdSpeed;
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ // if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
+ //Write out data
+ if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << currentTime<<" "<<cmdPosition<<" "<<currentAngle<<" "<<positionError<< " "<<currentCmd << std::endl;
+
+ // limit torque
+ currentCmd = (currentCmd > 100) ? 100: currentCmd;
+ currentCmd = (currentCmd < -100 ) ? -100: currentCmd;
+ //currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ //currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ hjoint->SetParam( dParamFMax, 0);
+ hjoint->SetTorque(currentCmd + dampForce);
+
+ break;
+ case PR2::SPEED_TORQUE_CONTROL :
+ currentRate = hjoint->GetAngleRate();
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->pids[count]->UpdatePid(speedError, currentTime-this->lastTime);
+ //if(count==PR2::ARM_R_PAN)std::cout<<"Joint:" <<count << " Desired:" << cmdSpeed << " Current speed"<<currentRate<<" Error"<<speedError<<" cmd: " << currentCmd << std::endl;
+ if(count==PR2::ARM_R_ELBOW_PITCH )std::cout<<currentTime<<" "<<currentRate<<" "<<speedError<<" " << currentCmd << std::endl;
+ // limit torque
+
+ currentCmd = (currentCmd > 100) ? 100: currentCmd;
+ currentCmd = (currentCmd < -100 ) ? -100: currentCmd;
+
+ //Needs to be set to 0 1x for every joint
+ hjoint->SetParam( dParamFMax, 0);
+ hjoint->SetTorque(currentCmd);
+
+
+ break;
+ case PR2::PD_CONTROL:
+ //if (cmdPosition > hjoint->GetHighStop())
+ // cmdPosition = hjoint->GetHighStop();
+ //else if (cmdPosition < hjoint->GetLowStop())
+ // cmdPosition = hjoint->GetLowStop();
+
+ currentAngle = hjoint->GetAngle();
+ currentRate = hjoint->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle - cmdPosition);
+ speedError = currentRate - cmdSpeed;
+ //std::cout << "hinge e:" << speedError << " + " << positionError << std::endl;
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ hjoint->SetParam( dParamVel, currentCmd );
+ hjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
+ break;
+ case PR2::SPEED_CONTROL:
+ // printf("Hinge Speed Control\n");
+ //std::cout << "wheel drive: " << cmdSpeed << " i " << count << std::endl;
+ hjoint->SetParam( dParamVel, cmdSpeed);
+ hjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ break;
+ case PR2::DISABLED:
+ hjoint->SetParam( dParamFMax, 0);
+ hjoint->SetTorque(0); //disable the joint
+ break;
+ default:
+ break;
+
+ }
+ this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
+ this->myIface->data->actuators[count].actualSpeed = hjoint->GetAngleRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ break;
+ case Joint::HINGE2:
+ case Joint::BALL:
+ case Joint::UNIVERSAL:
+ break;
+ } // end of switch block
+ } // end of if motor block
+ } // end of for-count block
+
this->myIface->data->new_cmd = 0;
this->myIface->Unlock();
this->lastTime = currentTime;
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-02 22:10:25 UTC (rev 2492)
@@ -77,115 +77,9 @@
- <body:box name="finger_1_right">
- <xyz>0.93 -0.172936 0.8</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_right_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
- <body:box name="finger_2_right">
- <xyz>0.93 -0.132936 0.8</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_right_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
- </geom:box>
- </body:box>
-
-
- <joint:slider name="finger_1_slider_right">
- <body2>finger_1_right</body2>
- <body1>gripper_roll_right</body1>
- <anchor>gripper_roll_right</anchor>
- <lowStop> -0.02 </lowStop> <!-- red close -->
- <highStop> 0.05 </highStop> <!-- red open -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <joint:slider name="finger_2_slider_right">
- <body2>finger_2_right</body2>
- <body1>gripper_roll_right</body1>
- <anchor>gripper_roll_right</anchor>
- <lowStop> -0.05 </lowStop> <!-- white open -->
- <highStop> 0.02 </highStop> <!-- white close -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <!-- ========== Left Arm =========== -->
-
- <body:box name="finger_1_left">
- <xyz>0.93 0.132936 0.8</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_left_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="finger_2_left">
- <xyz>0.93 0.172936 0.8</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_left_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
- </geom:box>
- </body:box>
-
- <joint:slider name="finger_1_slider_left">
- <body2>finger_1_left</body2>
- <body1>gripper_roll_left</body1>
- <anchor>gripper_roll_left</anchor>
- <lowStop> -0.02 </lowStop> <!-- red close -->
- <highStop> 0.05 </highStop> <!-- red open -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <joint:slider name="finger_2_slider_left">
- <body2>finger_2_left</body2>
- <body1>gripper_roll_left</body1>
- <anchor>gripper_roll_left</anchor>
- <lowStop> -0.05 </lowStop> <!-- white open -->
- <highStop> 0.02 </highStop> <!-- white close -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
-
-
-
-
<!-- left camera -->
<body:box name="ptz_cam_left_body">
<xyz> -0.1 0.30 1.04 </xyz>
@@ -400,7 +294,7 @@
<!-- ========================================== Hokuyo laser body ========================================== -->
<body:box name="base_laser_body">
- <xyz>0.25 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
+ <xyz>0.25 0.0 0.30</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
<rpy>0.0 0.0 0.0</rpy>
<geom:box name="base_laser_box">
@@ -457,6 +351,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="wheel_front_left_l_joint">
<saturationTorque>5</saturationTorque>
@@ -478,6 +374,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="wheel_front_right_l_joint">
<saturationTorque>5</saturationTorque>
@@ -499,6 +397,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="wheel_rear_left_l_joint">
<saturationTorque>5</saturationTorque>
@@ -520,6 +420,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="wheel_rear_right_l_joint">
<saturationTorque>5</saturationTorque>
@@ -543,6 +445,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<!-- ========================================= -->
<!-- left arm array -->
@@ -552,6 +456,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="shoulder_pitch_left_joint">
<saturationTorque>1000</saturationTorque>
@@ -559,6 +465,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="upperarm_roll_left_joint">
<saturationTorque>1000</saturationTorque>
@@ -566,6 +474,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="elbow_flex_left_joint">
<saturationTorque>100</saturationTorque>
@@ -573,6 +483,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="forearm_roll_left_joint">
<saturationTorque>100</saturationTorque>
@@ -580,6 +492,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="wrist_flex_left_joint">
<saturationTorque>100</saturationTorque>
@@ -587,6 +501,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="gripper_roll_left_joint">
<saturationTorque>100</saturationTorque>
@@ -594,7 +510,23 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
+ <!-- Special gripper joint -->
+ <joint name="gripper_left_joint" type="gripper_special">
+ <left_proximal>finger_l_left_joint</left_proximal>
+ <left_distal>finger_tip_l_left_joint</left_distal>
+ <right_proximal>finger_r_left_joint</right_proximal>
+ <right_distal>finger_tip_r_left_joint</right_distal>
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
+ </joint>
<!-- ========================================= -->
<!-- right arm array -->
<joint name="shoulder_pan_right_joint">
@@ -603,6 +535,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="shoulder_pitch_right_joint">
<saturationTorque>1000</saturationTorque>
@@ -610,6 +544,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="upperarm_roll_right_joint">
<saturationTorque>1000</saturationTorque>
@@ -617,6 +553,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="elbow_flex_right_joint">
<saturationTorque>1000</saturationTorque>
@@ -624,6 +562,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="forearm_roll_right_joint">
<saturationTorque>100</saturationTorque>
@@ -631,6 +571,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="wrist_flex_right_joint">
<saturationTorque>100</saturationTorque>
@@ -638,6 +580,8 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
<joint name="gripper_roll_right_joint">
<saturationTorque>100</saturationTorque>
@@ -645,7 +589,23 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <controlMode>PD_CONTROL</controlMode>
</joint>
+ <!-- Special gripper joint -->
+ <joint name="gripper_right_joint" type="gripper_special">
+ <left_proximal>finger_l_right_joint</left_proximal>
+ <left_distal>finger_tip_l_right_joint</left_distal>
+ <right_proximal>finger_r_right_joint</right_proximal>
+ <right_distal>finger_tip_r_right_joint</right_distal>
+ <saturationTorque>100</saturationTorque>
+ <pGain...
[truncated message content] |
