1
2 #include "KukaGraspExample.h"
3 #include "../SharedMemory/IKTrajectoryHelper.h"
4
5 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
6 #include "Bullet3Common/b3Quaternion.h"
7 #include "Bullet3Common/b3AlignedObjectArray.h"
8 #include "../CommonInterfaces/CommonRenderInterface.h"
9 #include "../CommonInterfaces/CommonExampleInterface.h"
10 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
11 #include "../SharedMemory/PhysicsServerSharedMemory.h"
12 #include "../SharedMemory/PhysicsClientC_API.h"
13 #include <string>
14 #include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
15
16 #include "../Utils/b3Clock.h"
17
18 ///quick demo showing the right-handed coordinate system and positive rotations around each axis
19 class KukaGraspExample : public CommonExampleInterface
20 {
21 CommonGraphicsApp* m_app;
22 GUIHelperInterface* m_guiHelper;
23 b3RobotSimulatorClientAPI m_robotSim;
24
25 int m_kukaIndex;
26
27 IKTrajectoryHelper m_ikHelper;
28 int m_targetSphereInstance;
29 b3Vector3 m_targetPos;
30 b3Vector3 m_worldPos;
31 b3Vector4 m_targetOri;
32 b3Vector4 m_worldOri;
33 double m_time;
34 // int m_options;
35
36 b3AlignedObjectArray<int> m_movingInstances;
37 enum
38 {
39 numCubesX = 20,
40 numCubesY = 20
41 };
42
43 public:
KukaGraspExample(GUIHelperInterface * helper,int)44 KukaGraspExample(GUIHelperInterface* helper, int /* options */)
45 : m_app(helper->getAppInterface()),
46 m_guiHelper(helper),
47 // m_options(options),
48 m_kukaIndex(-1),
49 m_time(0)
50 {
51 m_targetPos.setValue(0.5, 0, 1);
52 m_worldPos.setValue(0, 0, 0);
53 m_app->setUpAxis(2);
54 }
~KukaGraspExample()55 virtual ~KukaGraspExample()
56 {
57 }
58
initPhysics()59 virtual void initPhysics()
60 {
61 ///create some graphics proxy for the tracking target
62 ///the endeffector tries to track it using Inverse Kinematics
63 {
64 int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
65 b3Quaternion orn(0, 0, 0, 1);
66 b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
67 b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
68 m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
69 }
70 m_app->m_renderer->writeTransforms();
71
72 int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
73 m_robotSim.setGuiHelper(m_guiHelper);
74 bool connected = m_robotSim.connect(mode);
75 m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0);
76 m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
77 m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0);
78
79 // 0;//m_robotSim.connect(m_guiHelper);
80 b3Printf("robotSim connected = %d", connected);
81
82 {
83 m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf");
84 if (m_kukaIndex >= 0)
85 {
86 int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
87 b3Printf("numJoints = %d", numJoints);
88
89 for (int i = 0; i < numJoints; i++)
90 {
91 b3JointInfo jointInfo;
92 m_robotSim.getJointInfo(m_kukaIndex, i, &jointInfo);
93 b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
94 }
95 /*
96 int wheelJointIndices[4]={2,3,6,7};
97 int wheelTargetVelocities[4]={-10,-10,-10,-10};
98 for (int i=0;i<4;i++)
99 {
100 b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
101 controlArgs.m_targetVelocity = wheelTargetVelocities[i];
102 controlArgs.m_maxTorqueValue = 1e30;
103 m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs);
104 }
105 */
106 }
107
108 {
109 m_robotSim.loadURDF("plane.urdf");
110 m_robotSim.setGravity(btVector3(0, 0, 0));
111 }
112 }
113 }
exitPhysics()114 virtual void exitPhysics()
115 {
116 m_robotSim.disconnect();
117 }
stepSimulation(float deltaTime)118 virtual void stepSimulation(float deltaTime)
119 {
120 float dt = deltaTime;
121 btClamp(dt, 0.0001f, 0.01f);
122
123 m_time += dt;
124 m_targetPos.setValue(0.4 - 0.4 * b3Cos(m_time), 0, 0.8 + 0.4 * b3Cos(m_time));
125 m_targetOri.setValue(0, 1.0, 0, 0);
126 m_targetPos.setValue(0.2 * b3Cos(m_time), 0.2 * b3Sin(m_time), 1.1);
127
128 int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
129
130 if (numJoints == 7)
131 {
132 double q_current[7] = {0, 0, 0, 0, 0, 0, 0};
133
134 b3JointStates2 jointStates;
135
136 if (m_robotSim.getJointStates(m_kukaIndex, jointStates))
137 {
138 //skip the base positions (7 values)
139 b3Assert(7 + numJoints == jointStates.m_numDegreeOfFreedomQ);
140 for (int i = 0; i < numJoints; i++)
141 {
142 q_current[i] = jointStates.m_actualStateQ[i + 7];
143 }
144 }
145 // compute body position and orientation
146 b3LinkState linkState;
147 bool computeVelocity = true;
148 bool computeForwardKinematics = true;
149 m_robotSim.getLinkState(0, 6, computeVelocity, computeForwardKinematics, &linkState);
150
151 m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
152 m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
153
154 b3Vector3DoubleData targetPosDataOut;
155 m_targetPos.serializeDouble(targetPosDataOut);
156 b3Vector3DoubleData worldPosDataOut;
157 m_worldPos.serializeDouble(worldPosDataOut);
158 b3Vector3DoubleData targetOriDataOut;
159 m_targetOri.serializeDouble(targetOriDataOut);
160 b3Vector3DoubleData worldOriDataOut;
161 m_worldOri.serializeDouble(worldOriDataOut);
162
163 b3RobotSimulatorInverseKinematicArgs ikargs;
164 b3RobotSimulatorInverseKinematicsResults ikresults;
165
166 ikargs.m_bodyUniqueId = m_kukaIndex;
167 // ikargs.m_currentJointPositions = q_current;
168 // ikargs.m_numPositions = 7;
169 ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
170 ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
171 ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
172
173 //ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION;
174 ikargs.m_flags |= B3_HAS_JOINT_DAMPING;
175
176 ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
177 ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
178 ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
179 ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
180 ikargs.m_endEffectorLinkIndex = 6;
181
182 // Settings based on default KUKA arm setting
183 ikargs.m_lowerLimits.resize(numJoints);
184 ikargs.m_upperLimits.resize(numJoints);
185 ikargs.m_jointRanges.resize(numJoints);
186 ikargs.m_restPoses.resize(numJoints);
187 ikargs.m_jointDamping.resize(numJoints, 0.5);
188 ikargs.m_lowerLimits[0] = -2.32;
189 ikargs.m_lowerLimits[1] = -1.6;
190 ikargs.m_lowerLimits[2] = -2.32;
191 ikargs.m_lowerLimits[3] = -1.6;
192 ikargs.m_lowerLimits[4] = -2.32;
193 ikargs.m_lowerLimits[5] = -1.6;
194 ikargs.m_lowerLimits[6] = -2.4;
195 ikargs.m_upperLimits[0] = 2.32;
196 ikargs.m_upperLimits[1] = 1.6;
197 ikargs.m_upperLimits[2] = 2.32;
198 ikargs.m_upperLimits[3] = 1.6;
199 ikargs.m_upperLimits[4] = 2.32;
200 ikargs.m_upperLimits[5] = 1.6;
201 ikargs.m_upperLimits[6] = 2.4;
202 ikargs.m_jointRanges[0] = 5.8;
203 ikargs.m_jointRanges[1] = 4;
204 ikargs.m_jointRanges[2] = 5.8;
205 ikargs.m_jointRanges[3] = 4;
206 ikargs.m_jointRanges[4] = 5.8;
207 ikargs.m_jointRanges[5] = 4;
208 ikargs.m_jointRanges[6] = 6;
209 ikargs.m_restPoses[0] = 0;
210 ikargs.m_restPoses[1] = 0;
211 ikargs.m_restPoses[2] = 0;
212 ikargs.m_restPoses[3] = SIMD_HALF_PI;
213 ikargs.m_restPoses[4] = 0;
214 ikargs.m_restPoses[5] = -SIMD_HALF_PI * 0.66;
215 ikargs.m_restPoses[6] = 0;
216 ikargs.m_jointDamping[0] = 10.0;
217 ikargs.m_numDegreeOfFreedom = numJoints;
218
219 if (m_robotSim.calculateInverseKinematics(ikargs, ikresults))
220 {
221 //copy the IK result to the desired state of the motor/actuator
222 for (int i = 0; i < numJoints; i++)
223 {
224 b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
225 t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
226 t.m_maxTorqueValue = 100.0;
227 t.m_kp = 1.0;
228 t.m_targetVelocity = 0;
229 t.m_kd = 1.0;
230 m_robotSim.setJointMotorControl(m_kukaIndex, i, t);
231 }
232 }
233 }
234
235 m_robotSim.stepSimulation();
236 }
renderScene()237 virtual void renderScene()
238 {
239 m_robotSim.renderScene();
240
241 b3Quaternion orn(0, 0, 0, 1);
242
243 m_app->m_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance);
244 m_app->m_renderer->writeTransforms();
245
246 //draw the end-effector target sphere
247
248 //m_app->m_renderer->renderScene();
249 }
250
physicsDebugDraw(int debugDrawMode)251 virtual void physicsDebugDraw(int debugDrawMode)
252 {
253 m_robotSim.debugDraw(debugDrawMode);
254 }
mouseMoveCallback(float x,float y)255 virtual bool mouseMoveCallback(float x, float y)
256 {
257 return false;
258 }
mouseButtonCallback(int button,int state,float x,float y)259 virtual bool mouseButtonCallback(int button, int state, float x, float y)
260 {
261 return false;
262 }
keyboardCallback(int key,int state)263 virtual bool keyboardCallback(int key, int state)
264 {
265 return false;
266 }
267
resetCamera()268 virtual void resetCamera()
269 {
270 float dist = 3;
271 float pitch = -30;
272 float yaw = 0;
273 float targetPos[3] = {-0.2, 0.8, 0.3};
274 if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
275 {
276 m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
277 m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
278 m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
279 m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
280 }
281 }
282 };
283
KukaGraspExampleCreateFunc(struct CommonExampleOptions & options)284 class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options)
285 {
286 return new KukaGraspExample(options.m_guiHelper, options.m_option);
287 }
288