Modify shared memory client example to test joint motor torque measurement.

data/kuka_iiwa/model.urdf

@@ -0,0 +1,284 @@
+<?xml version="1.0" ?>
+<!-- ======================================================================= -->
+<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro   | -->
+<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED                        | -->
+<!-- | Changes (kohlhoff):                                                 | -->
+<!-- |   * Removed gazebo tags.                                            | -->
+<!-- |   * Removed unused materials.                                       | -->
+<!-- |   * Made mesh paths relative.                                       | -->
+<!-- |   * Removed material fields from collision segments.                | -->
+<!-- |   * Removed the self_collision_checking segment.                    | -->
+<!-- |   * Removed transmission segments, since they didn't match the      | -->
+<!-- |     convention, will have to added back later.                      | -->
+<!-- ======================================================================= -->
+<!--LICENSE:                                                                 -->
+<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center,  -->
+<!--Orebro University, Sweden                                                -->
+<!--All rights reserved.                                                     -->
+<!--                                                                         -->
+<!--Redistribution and use in source and binary forms, with or without       -->
+<!--modification, are permitted provided that the following conditions are   -->
+<!--met:                                                                     -->
+<!--                                                                         -->
+<!--1. Redistributions of source code must retain the above copyright notice,-->
+<!--   this list of conditions and the following disclaimer.                 -->
+<!--                                                                         -->
+<!--2. Redistributions in binary form must reproduce the above copyright     -->
+<!--   notice, this list of conditions and the following disclaimer in the   -->
+<!--   documentation and/or other materials provided with the distribution.  -->
+<!--                                                                         -->
+<!--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS  -->
+<!--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,-->
+<!--THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR   -->
+<!--PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR        -->
+<!--CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,    -->
+<!--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,      -->
+<!--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR       -->
+<!--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF   -->
+<!--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING     -->
+<!--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS       -->
+<!--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.             -->
+<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Import Rviz colors -->
+  <material name="Grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="Orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <!--Import the lbr iiwa macro -->
+  <!--Import Transmissions -->
+  <!--Include Utilities -->
+  <!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
+  <!--Little helper macros to define the inertia matrix needed for links.-->
+  <!--Cuboid-->
+  <!--Cylinder: length is along the y-axis! -->
+  <!--lbr-->
+  <link name="lbr_iiwa_link_0">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
+      <!--Increase mass from 5 Kg original to provide a stable base to carry the
+          arm.-->
+      <mass value="0.0"/>
+      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_0.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_0.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_0 and link_1 -->
+  <joint name="lbr_iiwa_joint_1" type="revolute">
+    <parent link="lbr_iiwa_link_0"/>
+    <child link="lbr_iiwa_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1575"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+      <mass value="4"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_1.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_1 and link_2 -->
+  <joint name="lbr_iiwa_joint_2" type="revolute">
+    <parent link="lbr_iiwa_link_1"/>
+    <child link="lbr_iiwa_link_2"/>
+    <origin rpy="1.57079632679   0 3.14159265359" xyz="0 0 0.2025"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
+      <mass value="4"/>
+      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_2.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_2 and link_3 -->
+  <joint name="lbr_iiwa_joint_3" type="revolute">
+    <parent link="lbr_iiwa_link_2"/>
+    <child link="lbr_iiwa_link_3"/>
+    <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
+      <mass value="3"/>
+      <inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_3.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_3 and link_4 -->
+  <joint name="lbr_iiwa_joint_4" type="revolute">
+    <parent link="lbr_iiwa_link_3"/>
+    <child link="lbr_iiwa_link_4"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
+      <mass value="2.7"/>
+      <inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_4.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_4 and link_5 -->
+  <joint name="lbr_iiwa_joint_5" type="revolute">
+    <parent link="lbr_iiwa_link_4"/>
+    <child link="lbr_iiwa_link_5"/>
+    <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
+      <mass value="1.7"/>
+      <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_5.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_5 and link_6 -->
+  <joint name="lbr_iiwa_joint_6" type="revolute">
+    <parent link="lbr_iiwa_link_5"/>
+    <child link="lbr_iiwa_link_6"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
+      <mass value="1.8"/>
+      <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_6.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joint between link_6 and link_7 -->
+  <joint name="lbr_iiwa_joint_7" type="revolute">
+    <parent link="lbr_iiwa_link_6"/>
+    <child link="lbr_iiwa_link_7"/>
+    <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="lbr_iiwa_link_7">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.02"/>
+      <mass value="0.3"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_7.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/coarse/link_7.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -219,11 +219,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
             color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
          }
          */
-        //btVector3 localInertiaDiagonal(0, 0, 0);
+        if (mass)
-        //if (mass)
+        {
-        //{
+        	compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
-        //	shape->calculateLocalInertia(mass, localInertiaDiagonal);
+        }
-        //}
 
         btRigidBody* linkRigidBody = 0;
         btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;

examples/SharedMemory/PhysicsClientExample.cpp

@@ -41,7 +41,7 @@ protected:
 public:
     
     //@todo, add accessor methods
-	MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
+	// MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
     MyMotorInfo2 m_motorTargetPositions[MAX_NUM_MOTORS];
 	int m_numMotors;
 
@@ -140,12 +140,15 @@ public:
         for (int i=0;i<m_numMotors;i++)
         {
             // btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
-            int uIndex = m_motorTargetVelocities[i].m_uIndex;
+            // int uIndex = m_motorTargetVelocities[i].m_uIndex;
             // b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
             
             btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
             int qIndex = m_motorTargetPositions[i].m_qIndex;
+            int uIndex = m_motorTargetPositions[i].m_uIndex;
             b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
+            b3JointControlSetKp(commandHandle, uIndex, 0.1);
+            b3JointControlSetKd(commandHandle, uIndex, 0.0);
             
             b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
         }
@@ -202,7 +205,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
         case  CMD_LOAD_URDF:
         {
             
-            b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_lwr/kuka.urdf");
+            b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
             
             //setting the initial position, orientation and other arguments are optional
             double startPosX = 0;
@@ -242,7 +245,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 			if (m_selectedBody>=0)
 			{
 				b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
+                b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
 				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+                
+                int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
+                for (int i = 0; i < numJoints; ++i) {
+                    struct b3JointSensorState sensorState;
+                    b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
+                    b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
+                }
 			}
             break;
         };
@@ -315,7 +326,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
         }
         case CMD_SEND_DESIRED_STATE:
         {
-            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
+            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
+            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
             prepareControlCommand(command);
             b3SubmitClientCommand(m_physicsClientHandle, command);
             break;
@@ -337,6 +349,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 
             break;
         }
+        case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
+            b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
+            b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
+            b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
+            break;
+        }
         default:
         {
             b3Error("Unknown buttonId");
@@ -394,6 +412,7 @@ void	PhysicsClientExample::createButtons()
 		createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
         createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
 		createButton("Initialize Pose",CMD_INIT_POSE,  isTrigger);
+        createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
 
 
 		if (m_physicsClientHandle && m_selectedBody>=0)
@@ -410,13 +429,18 @@ void	PhysicsClientExample::createButtons()
 					if (m_numMotors<MAX_NUM_MOTORS)
 					{
 						char motorName[1024];
-						sprintf(motorName,"%s q'", info.m_jointName);
+						sprintf(motorName,"%s q", info.m_jointName);
-						MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
+						// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
+                        MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
 						motorInfo->m_velTarget = 0.f;
+                        motorInfo->m_posTarget = 0.f;
 						motorInfo->m_uIndex = info.m_uIndex;
                         motorInfo->m_qIndex = info.m_qIndex;
                         
-						SliderParams slider(motorName,&motorInfo->m_velTarget);
+						// SliderParams slider(motorName,&motorInfo->m_velTarget);
+						// slider.m_minVal=-4;
+						// slider.m_maxVal=4;
+                        SliderParams slider(motorName,&motorInfo->m_posTarget);
                         slider.m_minVal=-4;
                         slider.m_maxVal=4;
 						if (m_guiHelper && m_guiHelper->getParameterInterface())