Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add IK damping coefficient vector.

Browse Source
This commit is contained in:
YunfeiBai
2016-09-27 16:37:49 -07:00
parent 61fccba5be
commit 3910003e0a
6 changed files with 15 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
data/gripper/wsg50_one_motor_gripper_new.sdf → data/gripper/wsg50_one_motor_gripper_free_base.sdf
View File

61
data/table.urdf
View File

@@ -1,61 +0,0 @@
<?xml version="0.0" ?>
<robot name="urdf_table">
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="baseLink"/>
<origin xyz="0 0 0"/>
</joint>
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.6"/>
<mass value="0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.6"/>
<geometry>
<box size="1.5 1.0 0.08"/>
</geometry>
<material name="framemat0">
<color
rgba="1 1 1 1" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.65 -0.4 0.28"/>
<geometry>
<box size="0.1 0.1 0.56"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.65 0.4 0.28"/>
<geometry>
<box size="0.1 0.1 0.56"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.65 -0.4 0.28"/>
<geometry>
<box size="0.1 0.1 0.56"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.65 0.4 0.28"/>
<geometry>
<box size="0.1 0.1 0.56"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.6"/>
<geometry>
<box size="1.5 1.0 0.08"/>
</geometry>
</collision>
</link>
</robot>

91
data/tray.urdf
View File

@@ -1,91 +0,0 @@
<?xml version="1.0"?>
<robot name="tray">
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.29 0.0 0.05"/>
<geometry>
<box size="0.02 0.6 0.06"/>
</geometry>
<material name="framemat0">
<color rgba="0.6 0.6 0.6 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.29 0.0 0.05"/>
<geometry>
<box size="0.02 0.6 0.06"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0.29 0.0 0.05"/>
<geometry>
<box size="0.02 0.6 0.06"/>
</geometry>
<material name="framemat0">
<color rgba="0.6 0.6 0.6 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.29 0.0 0.05"/>
<geometry>
<box size="0.02 0.6 0.06"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0.0 -0.29 0.05"/>
<geometry>
<box size="0.56 0.02 0.06"/>
</geometry>
<material name="framemat0">
<color rgba="0.6 0.6 0.6 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 -0.29 0.05"/>
<geometry>
<box size="0.56 0.02 0.06"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0.0 0.29 0.05"/>
<geometry>
<box size="0.56 0.02 0.06"/>
</geometry>
<material name="framemat0">
<color rgba="0.6 0.6 0.6 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.29 0.05"/>
<geometry>
<box size="0.56 0.02 0.06"/>
</geometry>
</collision>
<!-- base -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<geometry>
<box size="0.6 0.6 0.02"/>
</geometry>
<material name="framemat0"/>
<color rgba="0.4 0.4 0.4 1"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<geometry>
<box size="0.6 0.6 0.02"/>
</geometry>
</collision>
</link>
</robot>

9
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -43,7 +43,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk)
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false;
@@ -69,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,dampIk*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
@@ -79,13 +79,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle*dampIk;
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,angularVel[i]);
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
deltaR[2] = 0.0;
{

13
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -21,13 +21,12 @@ public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk=1.);
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
};
#endif //IK_TRAJECTORY_HELPER_H

19
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2650,12 +2650,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
double dampIK[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2);
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIK);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
@@ -2941,11 +2941,11 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
}
loadUrdf("kuka_iiwa/model.urdf", btVector3(0, -2.3, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(0, -3.0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadSdf("gripper/wsg50_one_motor_gripper_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
@@ -3013,15 +3013,6 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Table area
loadUrdf("table.urdf", btVector3(0, -1.9, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray.urdf", btVector3(0, -1.7, 0.64), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cup_small.urdf", btVector3(0.5, -2.0, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("pitcher_small.urdf", btVector3(0.4, -1.8, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, -1.9, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.1, -1.8, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.2, -1.7, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
@@ -3105,7 +3096,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
if (closeToKuka)
{
int dampIk = 1;
double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0};
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;