mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
Modify shared memory client example to test joint motor torque measurement.
This commit is contained in:
yunfeibai
parent
c384383250
commit
cbeddfc897
BIN
data/kuka_iiwa/meshes/coarse/link_0.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_0.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_1.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_1.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_2.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_2.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_3.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_3.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_4.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_4.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_5.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_5.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_6.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_6.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/coarse/link_7.stl
Normal file
BIN
data/kuka_iiwa/meshes/coarse/link_7.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_0.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_0.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_1.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_1.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_2.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_2.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_3.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_3.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_4.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_4.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_5.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_5.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_6.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_6.stl
Normal file
Binary file not shown.
BIN
data/kuka_iiwa/meshes/link_7.stl
Normal file
BIN
data/kuka_iiwa/meshes/link_7.stl
Normal file
Binary file not shown.
284
data/kuka_iiwa/model.urdf
Normal file
284
data/kuka_iiwa/model.urdf
Normal file
@ -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>
|
||||||
|
|
||||||
@ -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);
|
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;
|
btRigidBody* linkRigidBody = 0;
|
||||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
|
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
|
||||||
|
|||||||
@ -41,7 +41,7 @@ protected:
|
|||||||
public:
|
public:
|
||||||
|
|
||||||
//@todo, add accessor methods
|
//@todo, add accessor methods
|
||||||
MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
|
// MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
|
||||||
MyMotorInfo2 m_motorTargetPositions[MAX_NUM_MOTORS];
|
MyMotorInfo2 m_motorTargetPositions[MAX_NUM_MOTORS];
|
||||||
int m_numMotors;
|
int m_numMotors;
|
||||||
|
|
||||||
@ -140,12 +140,15 @@ public:
|
|||||||
for (int i=0;i<m_numMotors;i++)
|
for (int i=0;i<m_numMotors;i++)
|
||||||
{
|
{
|
||||||
// btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
|
// 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);
|
// b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
|
||||||
|
|
||||||
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
|
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
|
||||||
int qIndex = m_motorTargetPositions[i].m_qIndex;
|
int qIndex = m_motorTargetPositions[i].m_qIndex;
|
||||||
|
int uIndex = m_motorTargetPositions[i].m_uIndex;
|
||||||
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
|
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
|
||||||
|
b3JointControlSetKp(commandHandle, uIndex, 0.1);
|
||||||
|
b3JointControlSetKd(commandHandle, uIndex, 0.0);
|
||||||
|
|
||||||
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
|
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
|
||||||
}
|
}
|
||||||
@ -202,7 +205,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
|||||||
case CMD_LOAD_URDF:
|
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
|
//setting the initial position, orientation and other arguments are optional
|
||||||
double startPosX = 0;
|
double startPosX = 0;
|
||||||
@ -242,7 +245,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
|||||||
if (m_selectedBody>=0)
|
if (m_selectedBody>=0)
|
||||||
{
|
{
|
||||||
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
|
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
|
||||||
|
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
|
||||||
b3SubmitClientCommand(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;
|
break;
|
||||||
};
|
};
|
||||||
@ -315,7 +326,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
|||||||
}
|
}
|
||||||
case CMD_SEND_DESIRED_STATE:
|
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);
|
prepareControlCommand(command);
|
||||||
b3SubmitClientCommand(m_physicsClientHandle, command);
|
b3SubmitClientCommand(m_physicsClientHandle, command);
|
||||||
break;
|
break;
|
||||||
@ -337,6 +349,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
|||||||
|
|
||||||
break;
|
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:
|
default:
|
||||||
{
|
{
|
||||||
b3Error("Unknown buttonId");
|
b3Error("Unknown buttonId");
|
||||||
@ -394,6 +412,7 @@ void PhysicsClientExample::createButtons()
|
|||||||
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
|
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
|
||||||
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
|
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
|
||||||
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
|
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
|
||||||
|
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
|
||||||
|
|
||||||
|
|
||||||
if (m_physicsClientHandle && m_selectedBody>=0)
|
if (m_physicsClientHandle && m_selectedBody>=0)
|
||||||
@ -410,15 +429,20 @@ void PhysicsClientExample::createButtons()
|
|||||||
if (m_numMotors<MAX_NUM_MOTORS)
|
if (m_numMotors<MAX_NUM_MOTORS)
|
||||||
{
|
{
|
||||||
char motorName[1024];
|
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_velTarget = 0.f;
|
||||||
|
motorInfo->m_posTarget = 0.f;
|
||||||
motorInfo->m_uIndex = info.m_uIndex;
|
motorInfo->m_uIndex = info.m_uIndex;
|
||||||
motorInfo->m_qIndex = info.m_qIndex;
|
motorInfo->m_qIndex = info.m_qIndex;
|
||||||
|
|
||||||
SliderParams slider(motorName,&motorInfo->m_velTarget);
|
// SliderParams slider(motorName,&motorInfo->m_velTarget);
|
||||||
slider.m_minVal=-4;
|
// slider.m_minVal=-4;
|
||||||
slider.m_maxVal=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())
|
if (m_guiHelper && m_guiHelper->getParameterInterface())
|
||||||
{
|
{
|
||||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user