|
From: <hsu...@us...> - 2008-08-22 00:51:43
|
Revision: 3456
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3456&view=rev
Author: hsujohnhsu
Date: 2008-08-22 00:51:53 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
kp_speed changed to match that of the hardware (10.0), and base controller is behaving better.
update some comments in test actuator.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-08-22 00:49:26 UTC (rev 3455)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-08-22 00:51:53 UTC (rev 3456)
@@ -898,31 +898,92 @@
this->lock.unlock();
}
+
void
+ TestActuators::CmdBaseVelReceived()
+ {
+ this->lock.lock();
+ controller::Controller* cc = mc_.getControllerByName( "base_controller" );
+ controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
+ bc->setCommand(velMsg.vx,0.0,velMsg.vw);
+ this->lock.unlock();
+ }
+
+
+
+
+
+ void
TestActuators::CmdLeftarmcartesianReceived()
{
this->lock.lock();
+
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++) {
+ f.M.data[i] = cmd_leftarmcartesian.rot[i];
+ }
+ for(int i = 0; i < 3; i++) {
+ f.p.data[i] = cmd_leftarmcartesian.trans[i];
+ }
+
+ bool reachable;
+ //this->PR2Copy->SetArmCartesianPosition(PR2::PR2_LEFT_ARM,f, reachable);
+
this->lock.unlock();
}
void
TestActuators::CmdRightarmcartesianReceived()
{
this->lock.lock();
+
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++) {
+ f.M.data[i] = cmd_rightarmcartesian.rot[i];
+ }
+ for(int i = 0; i < 3; i++) {
+ f.p.data[i] = cmd_rightarmcartesian.trans[i];
+ }
+
+ bool reachable;
+ //this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f, reachable);
+
this->lock.unlock();
}
+ bool TestActuators::SetRightArmCartesian(rosgazebo::MoveCartesian::request &req, rosgazebo::MoveCartesian::response &res)
+ {
+ this->lock.lock();
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = req.e.rot[i];
- void
- TestActuators::CmdBaseVelReceived()
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = req.e.trans[i];
+
+ bool reachable;
+ this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,reachable);
+ res.reachable = (reachable==false) ? -1 : 0;
+
+ this->lock.unlock();
+ return true;
+ }
+
+ bool TestActuators::OperateRightGripper(rosgazebo::GripperCmd::request &req, rosgazebo::GripperCmd::response &res)
{
+ this->lock.lock();
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP, req.gap, 0);
+ this->lock.unlock();
+ return true;
+ }
+
+ bool TestActuators::reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res)
+ {
this->lock.lock();
- controller::Controller* cc = mc_.getControllerByName( "base_controller" );
- controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
- bc->setCommand(velMsg.vx,0.0,velMsg.vw);
+ this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, *(this->PR2Copy->right_arm_chain_->q_IK_guess));
this->lock.unlock();
+ return true;
}
-
void
TestActuators::PublishFrameTransforms()
{
@@ -941,11 +1002,12 @@
controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
bc->getOdometry(x,y,yaw,vx,vy,vyaw);
+ // FIXME: z, roll, pitch not accounted for
tfs->sendInverseEuler("FRAMEID_ODOM",
"base",
x,
y,
- 0, //z - base_center_offset_z, /* get infor from xml: half height of base box */
+ z, //z - base_center_offset_z, /* get infor from xml: half height of base box */
yaw,
pitch,
roll,
@@ -979,7 +1041,7 @@
"base",
base_torso_offset_x,
base_torso_offset_y,
- spine_height+base_torso_offset_z, /* FIXME: spine elevator not accounted for */
+ spine_height+base_torso_offset_z,
0.0,
0.0,
0.0,
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-22 00:49:26 UTC (rev 3455)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-22 00:51:53 UTC (rev 3456)
@@ -5,7 +5,7 @@
<controller name="base_controller" topic="base_controller" type="BaseControllerNode">
<map name="velocity_control" flag="base_control">
- <elem key="kp_speed">1.0</elem>
+ <elem key="kp_speed">10.0</elem>
</map>
<controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
<joint name="caster_front_left_joint" >
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|