From 587e7b9abd5541deb69b31e6a276c55cab3636af Mon Sep 17 00:00:00 2001 From: Sam Wenke Date: Sat, 3 Feb 2018 12:51:43 -0500 Subject: [PATCH 1/7] pybullet support for gym.Env, including v0.9.x --- .../pybullet_envs/bullet/cartpole_bullet.py | 9 ++- .../gym/pybullet_envs/bullet/kukaCamGymEnv.py | 46 +++++++------- .../gym/pybullet_envs/bullet/kukaGymEnv.py | 61 +++++++++---------- .../bullet/minitaur_duck_gym_env.py | 5 ++ .../pybullet_envs/bullet/minitaur_gym_env.py | 5 ++ .../gym/pybullet_envs/bullet/racecarGymEnv.py | 37 ++++++----- .../pybullet_envs/bullet/racecarZEDGymEnv.py | 35 ++++++----- .../bullet/simpleHumanoidGymEnv.py | 21 ++++--- .../pybullet/gym/pybullet_envs/env_bases.py | 18 +++++- 9 files changed, 140 insertions(+), 97 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py index e28a9e962..277e52ba2 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py @@ -64,10 +64,10 @@ class CartPoleBulletEnv(gym.Env): # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state - + dv = 0.1 deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] - + p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) done = x < -self.x_threshold \ @@ -99,3 +99,8 @@ class CartPoleBulletEnv(gym.Env): def _render(self, mode='human', close=False): return + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py index 4e7d7e70e..8ed171be0 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py @@ -56,8 +56,8 @@ class KukaCamGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -74,15 +74,15 @@ class KukaCamGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.5 +0.2*random.random() ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -98,7 +98,7 @@ class KukaCamGymEnv(gym.Env): return [seed] def getExtendedObservation(self): - + #camEyePos = [0.03,0.236,0.54] #distance = 1.06 #pitch=-56 @@ -118,13 +118,13 @@ class KukaCamGymEnv(gym.Env): viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0] #projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] - + img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) rgb=img_arr[2] np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.01 @@ -142,7 +142,7 @@ class KukaCamGymEnv(gym.Env): realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -158,11 +158,11 @@ class KukaCamGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() reward = self._reward() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -190,18 +190,18 @@ class KukaCamGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -212,7 +212,7 @@ class KukaCamGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -227,18 +227,18 @@ class KukaCamGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) - reward = -1000 + reward = -1000 numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -254,3 +254,7 @@ class KukaCamGymEnv(gym.Env): #print(reward) return reward + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index b9ef16017..6fa57e76a 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -44,7 +44,7 @@ class KukaGymEnv(gym.Env): self._maxSteps = maxSteps self.terminated = 0 self._cam_dist = 1.3 - self._cam_yaw = 180 + self._cam_yaw = 180 self._cam_pitch = -40 self._p = p @@ -61,8 +61,8 @@ class KukaGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([largeValObservation] * observationDim) + + observation_high = np.array([largeValObservation] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -80,15 +80,15 @@ class KukaGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.55 +0.12*random.random() ypos = 0 +0.2*random.random() ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -115,7 +115,7 @@ class KukaGymEnv(gym.Env): dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]] dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]] dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]] - + gripperEul = p.getEulerFromQuaternion(gripperOrn) #print("gripperEul") #print(gripperEul) @@ -126,17 +126,17 @@ class KukaGymEnv(gym.Env): #print(projectedBlockPos2D) #print("blockEulerInGripper") #print(blockEulerInGripper) - + #we return the relative x,y position and euler angle of block in gripper space blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]] - + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) - + self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.005 @@ -154,7 +154,7 @@ class KukaGymEnv(gym.Env): f = 0.3 realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -168,7 +168,7 @@ class KukaGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]]) actionCost = np.linalg.norm(npaction)*10. @@ -177,9 +177,9 @@ class KukaGymEnv(gym.Env): reward = self._reward()-actionCost #print("reward") #print(reward) - + #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode="rgb_array", close=False): @@ -208,18 +208,18 @@ class KukaGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>self._maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("terminating, closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -230,7 +230,7 @@ class KukaGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -245,19 +245,19 @@ class KukaGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) reward = -1000 - + numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -276,10 +276,7 @@ class KukaGymEnv(gym.Env): #print(reward) return reward - def reset(self): - """Resets the state of the environment and returns an initial observation. - - Returns: observation (object): the initial observation of the - space. - """ - return self._reset() \ No newline at end of file + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py index 44b41dd7d..b5c3375b4 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py @@ -385,3 +385,8 @@ class MinitaurBulletDuckEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py index 38ec631a5..158128067 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -376,3 +376,8 @@ class MinitaurBulletEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index 7537d54c1..bdde5d963 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -50,8 +50,8 @@ class RacecarGymEnv(gym.Env): #self.reset() observationDim = 2 #len(self.getExtendedObservation()) #print("observationDim") - #print(observationDim) - # observation_high = np.array([np.finfo(np.float32).max] * observationDim) + #print(observationDim) + # observation_high = np.array([np.finfo(np.float32).max] * observationDim) observation_high = np.ones(observationDim) * 1000 #np.inf if (isDiscrete): self.action_space = spaces.Discrete(9) @@ -59,7 +59,7 @@ class RacecarGymEnv(gym.Env): action_dim = 2 self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) - self.action_space = spaces.Box(-action_high, action_high) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -74,14 +74,14 @@ class RacecarGymEnv(gym.Env): # pos,orn = self._p.getBasePositionAndOrientation(i) # newpos = [pos[0],pos[1],pos[2]-0.1] # self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -101,18 +101,18 @@ class RacecarGymEnv(gym.Env): def getExtendedObservation(self): self._observation = [] #self._racecar.getObservation() carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) - + self._observation.extend([ballPosInCar[0],ballPosInCar[1]]) return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + if (self._isDiscrete): fwd = [-1,-1,-1,0,0,0,1,1,1] steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] @@ -128,14 +128,14 @@ class RacecarGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -159,13 +159,13 @@ class RacecarGymEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array - + def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -174,3 +174,8 @@ class RacecarGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index d9546de8c..277453e14 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -40,7 +40,7 @@ class RacecarZEDGymEnv(gym.Env): self._renders = renders self._width = 100 self._height = 10 - + self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient( @@ -53,8 +53,8 @@ class RacecarZEDGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (isDiscrete): self.action_space = spaces.Discrete(9) else: @@ -77,14 +77,14 @@ class RacecarZEDGymEnv(gym.Env): pos,orn = self._p.getBasePositionAndOrientation(i) newpos = [pos[0],pos[1],pos[2]+0.1] self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2red.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -104,13 +104,13 @@ class RacecarZEDGymEnv(gym.Env): def getExtendedObservation(self): carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) carmat = self._p.getMatrixFromQuaternion(carorn) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) dist0 = 0.3 dist1 = 1. eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3] - targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] + targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] up = [carmat[2],carmat[5],carmat[8]] viewMat = self._p.computeViewMatrix(eyePos,targetPos,up) #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) @@ -122,12 +122,12 @@ class RacecarZEDGymEnv(gym.Env): np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + if (self._isDiscrete): fwd = [-1,-1,-1,0,0,0,1,1,1] steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] @@ -143,14 +143,14 @@ class RacecarZEDGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -177,10 +177,10 @@ class RacecarZEDGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -189,3 +189,8 @@ class RacecarZEDGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py index c632e7f31..53b19b7a8 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py @@ -46,8 +46,8 @@ class SimpleHumanoidGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -57,14 +57,14 @@ class SimpleHumanoidGymEnv(gym.Env): #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf")) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + p.setGravity(0,0,-10) self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -82,7 +82,7 @@ class SimpleHumanoidGymEnv(gym.Env): def getExtendedObservation(self): self._observation = self._humanoid.getObservation() return self._observation - + def _step(self, action): self._humanoid.applyAction(action) for i in range(self._actionRepeat): @@ -96,7 +96,7 @@ class SimpleHumanoidGymEnv(gym.Env): reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -104,8 +104,13 @@ class SimpleHumanoidGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): reward=self._humanoid.distance print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 5d49c22c3..ceafb6404 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -43,7 +43,7 @@ class MJCFBaseBulletEnv(gym.Env): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False - + self.physicsClientId = 0 else: self.ownsPhysicsClient = True @@ -75,12 +75,12 @@ class MJCFBaseBulletEnv(gym.Env): self.isRender = True if mode != "rgb_array": return np.array([]) - + base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz - + view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -100,6 +100,7 @@ class MJCFBaseBulletEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array + def _close(self): if (self.ownsPhysicsClient): if (self.physicsClientId>=0): @@ -109,6 +110,17 @@ class MJCFBaseBulletEnv(gym.Env): def HUD(self, state, a, done): pass + # backwards compatibility for gym >= v0.9.x + # for extension of this class. + def step(self, *args, **kwargs): + return self._step(*args, **kwargs) + + close = _close + render = _render + reset = _reset + seed = _seed + + class Camera: def __init__(self): pass From 7f535367efc7d25dcb27bb7eb80c4cb358d1c78c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Feb 2018 16:19:53 -0800 Subject: [PATCH 2/7] Add new methods for b3RobotSimulatorClientAPI, thanks to John F. Canny for the contribution! --- .../b3RobotSimulatorClientAPI.cpp | 1101 +++++++++++++++-- .../b3RobotSimulatorClientAPI.h | 380 +++++- 2 files changed, 1365 insertions(+), 116 deletions(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 1a7bf5b77..9bc1f91c0 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -20,6 +20,22 @@ #include "../SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" +static void scalarToDouble3(b3Scalar a[3], double b[3]) +{ + for (int i = 0; i < 3; i++) + { + b[i] = a[i]; + } +} + +static void scalarToDouble4(b3Scalar a[4], double b[4]) +{ + for (int i = 0; i < 4; i++) + { + b[i] = a[i]; + } +} + struct b3RobotSimulatorClientAPI_InternalData { b3PhysicsClientHandle m_physicsClientHandle; @@ -82,7 +98,7 @@ bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y) } if (m_data->m_guiHelper) { - return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y); + return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y)!=0; } return false; } @@ -95,7 +111,7 @@ bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float } if (m_data->m_guiHelper) { - return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y); + return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y)!=0; } return false; } @@ -118,13 +134,13 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i switch (mode) { - case eCONNECT_EXISTING_EXAMPLE_BROWSER: + case eCONNECT_EXISTING_EXAMPLE_BROWSER: { sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper); break; } - case eCONNECT_GUI: + case eCONNECT_GUI: { int argc = 0; char* argv[1] = {0}; @@ -135,7 +151,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i #endif break; } - case eCONNECT_GUI_SERVER: + case eCONNECT_GUI_SERVER: { int argc = 0; char* argv[1] = {0}; @@ -146,12 +162,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i #endif break; } - case eCONNECT_DIRECT: + case eCONNECT_DIRECT: { sm = b3ConnectPhysicsDirect(); break; } - case eCONNECT_SHARED_MEMORY: + case eCONNECT_SHARED_MEMORY: { if (portOrKey >= 0) { @@ -160,7 +176,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i sm = b3ConnectSharedMemory(key); break; } - case eCONNECT_UDP: + case eCONNECT_UDP: { if (portOrKey >= 0) { @@ -175,7 +191,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i break; } - case eCONNECT_TCP: + case eCONNECT_TCP: { if (portOrKey >= 0) { @@ -190,7 +206,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i break; } - default: + default: { b3Warning("connectPhysicsServer unexpected argument"); } @@ -307,7 +323,7 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) b3SharedMemoryStatusHandle statusHandle; b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); -// b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + // b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) @@ -344,8 +360,8 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc b3LoadUrdfCommandSetFlags(command,args.m_flags); b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); + args.m_startPosition[1], + args.m_startPosition[2]); b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); if (args.m_forceOverrideFixedBase) { @@ -520,7 +536,7 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], - baseOrientation[2], baseOrientation[3]); + baseOrientation[2], baseOrientation[3]); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); @@ -548,9 +564,9 @@ bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& bas } b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, 0, - &actualStateQdot, 0 /* joint_reaction_forces */); + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); baseLinearVelocity[0] = actualStateQdot[0]; baseLinearVelocity[1] = actualStateQdot[1]; @@ -734,54 +750,54 @@ bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& } b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (statusHandle) - { - // double rootInertialFrame[7]; - const double* rootLocalInertialFrame; - const double* actualStateQ; - const double* actualStateQdot; - const double* jointReactionForces; - - int stat = b3GetStatusActualState(statusHandle, - &state.m_bodyUniqueId, - &state.m_numDegreeOfFreedomQ, - &state.m_numDegreeOfFreedomU, - &rootLocalInertialFrame, - &actualStateQ, - &actualStateQdot, - &jointReactionForces); - if (stat) - { - state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); - state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - for (int i=0;im_physicsClientHandle, bodyUniqueId); b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, - targetValue); + targetValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); return false; @@ -820,7 +836,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint b3SharedMemoryStatusHandle statusHandle; switch (args.m_controlMode) { - case CONTROL_MODE_VELOCITY: + case CONTROL_MODE_VELOCITY: { b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); b3JointInfo jointInfo; @@ -832,7 +848,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } - case CONTROL_MODE_POSITION_VELOCITY_PD: + case CONTROL_MODE_POSITION_VELOCITY_PD: { b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); b3JointInfo jointInfo; @@ -848,7 +864,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } - case CONTROL_MODE_TORQUE: + case CONTROL_MODE_TORQUE: { b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); b3JointInfo jointInfo; @@ -858,7 +874,7 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } - default: + default: { b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl"); } @@ -962,16 +978,16 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS int numPos = 0; bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0) != 0; + &results.m_bodyUniqueId, + &numPos, + 0) != 0; if (result && numPos) { results.m_calculatedJointPositions.resize(numPos); result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0]) != 0; + &results.m_bodyUniqueId, + &numPos, + &results.m_calculatedJointPositions[0]) != 0; } return result; } @@ -995,18 +1011,26 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, return false; } -bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) +bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) { - if (!isConnected()) - { + if (!isConnected()) { b3Warning("Not connected"); return false; } b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + if (computeLinkVelocity) { + b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity); + } + + if (computeForwardKinematics) { + b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics); + } + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { + + if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); return true; } @@ -1117,13 +1141,13 @@ void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); if (commandHandle) { - if ((cameraDistance >= 0)) - { - b3Vector3FloatData camTargetPos; - targetPos.serializeFloat(camTargetPos); - b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); - } - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + if ((cameraDistance >= 0)) + { + b3Vector3FloatData camTargetPos; + targetPos.serializeFloat(camTargetPos); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } } @@ -1151,9 +1175,902 @@ void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double return; } - b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSoftBodySetScale(command, scale); - b3LoadSoftBodySetMass(command, mass); - b3LoadSoftBodySetCollisionMargin(command, collisionMargin); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSoftBodySetScale(command, scale); + b3LoadSoftBodySetMass(command, mass); + b3LoadSoftBodySetCollisionMargin(command, collisionMargin); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); } + +void b3RobotSimulatorClientAPI::getMouseEvents(b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = 0; + mouseEventsData->m_mouseEvents = 0; + if (!isConnected()) { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData); +} + + +bool b3RobotSimulatorClientAPI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, + double *jointAccelerations, double *jointForcesOutput) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + + int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, + jointVelocities, jointAccelerations); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0); + + if (dofCount) { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput); + return true; + } + } + return false; +} + +int b3RobotSimulatorClientAPI::getNumBodies() const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + return b3GetNumBodies(m_data->m_physicsClientHandle); +} + +int b3RobotSimulatorClientAPI::getBodyUniqueId(int bodyId) const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId); +} + +bool b3RobotSimulatorClientAPI::removeBody(int bodyUniqueId) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId)); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_REMOVE_BODY_COMPLETED) { + return true; + } else { + b3Warning("getDynamicsInfo did not complete"); + return false; + } + } + b3Warning("removeBody could not submit command"); + return false; +} + +bool b3RobotSimulatorClientAPI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) { + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + // struct b3DynamicsInfo info; + + if (bodyUniqueId < 0) { + b3Warning("getDynamicsInfo failed; invalid bodyUniqueId"); + return false; + } + if (linkIndex < -1) { + b3Warning("getDynamicsInfo failed; invalid linkIndex"); + return false; + } + + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { + cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex); + status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) { + return true; + } else { + b3Warning("getDynamicsInfo did not complete"); + return false; + } + } + b3Warning("getDynamicsInfo could not submit command"); + return false; +} + + +bool b3RobotSimulatorClientAPI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (args.m_mass >= 0) { + b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass); + } + + if (args.m_lateralFriction >= 0) { + b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction); + } + + if (args.m_spinningFriction>=0) { + b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction); + } + + if (args.m_rollingFriction>=0) { + b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction); + } + + if (args.m_linearDamping>=0) { + b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping); + } + + if (args.m_angularDamping>=0) { + b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping); + } + + if (args.m_restitution>=0) { + b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution); + } + + if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) { + b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping); + } + + if (args.m_frictionAnchor>=0) { + b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +int b3RobotSimulatorClientAPI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugParameter failed."); + return -1; +} + + +double b3RobotSimulatorClientAPI::readUserDebugParameter(int itemUniqueId) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return 0; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) { + double paramValue = 0.f; + int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue); + if (ok) { + return paramValue; + } + } + b3Warning("readUserDebugParameter failed."); + return 0; +} + + +bool b3RobotSimulatorClientAPI::removeUserDebugItem(int itemUniqueId) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + return true; +} + + +int b3RobotSimulatorClientAPI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime); + + if (args.m_parentObjectUniqueId>=0) { + b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); + } + + if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) { + b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugText3D failed."); + return -1; +} + +int b3RobotSimulatorClientAPI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +{ + double dposXYZ[3]; + dposXYZ[0] = posXYZ.x; + dposXYZ[1] = posXYZ.y; + dposXYZ[2] = posXYZ.z; + + return addUserDebugText(text, &dposXYZ[0], args); +} + +int b3RobotSimulatorClientAPI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime); + + if (args.m_parentObjectUniqueId>=0) { + b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugLine failed."); + return -1; +} + +int b3RobotSimulatorClientAPI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +{ + double dfromXYZ[3]; + double dtoXYZ[3]; + dfromXYZ[0] = fromXYZ.x; + dfromXYZ[1] = fromXYZ.y; + dfromXYZ[2] = fromXYZ.z; + + dtoXYZ[0] = toXYZ.x; + dtoXYZ[1] = toXYZ.y; + dtoXYZ[2] = toXYZ.z; + return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args); +} + + + +bool b3RobotSimulatorClientAPI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + // int statusType; + struct b3JointInfo info; + + commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode); + + for (int i=0;im_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (args.m_numSolverIterations >= 0) { + b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations); + } + + if (args.m_collisionFilterMode >= 0) { + b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode); + } + + if (args.m_numSubSteps >= 0) { + b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps); + } + + if (args.m_fixedTimeStep >= 0) { + b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep); + } + + if (args.m_useSplitImpulse >= 0) { + b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse); + } + + if (args.m_splitImpulsePenetrationThreshold >= 0) { + b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold); + } + + if (args.m_contactBreakingThreshold >= 0) { + b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold); + } + + if (args.m_maxNumCmdPer1ms >= -1) { + b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms); + } + + if (args.m_restitutionVelocityThreshold>=0) { + b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold); + } + + if (args.m_enableFileCaching>=0) { + b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching); + } + + if (args.m_erp>=0) { + b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp); + } + + if (args.m_contactERP>=0) { + b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP); + } + + if (args.m_frictionERP >=0) { + b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + + +bool b3RobotSimulatorClientAPI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +bool b3RobotSimulatorClientAPI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags) +{ + double dforce[3]; + double dposition[3]; + + dforce[0] = force.x; + dforce[1] = force.y; + dforce[2] = force.z; + + dposition[0] = position.x; + dposition[1] = position.y; + dposition[2] = position.z; + + return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags); +} + + +bool b3RobotSimulatorClientAPI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +bool b3RobotSimulatorClientAPI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags) +{ + double dtorque[3]; + + dtorque[0] = torque.x; + dtorque[1] = torque.y; + dtorque[2] = torque.z; + + return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags); +} + + +bool b3RobotSimulatorClientAPI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex < 0) || (jointIndex >= numJoints)) { + b3Warning("Error: invalid jointIndex."); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3CreateSensorCommandInit(sm, bodyUniqueId); + b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CLIENT_COMMAND_COMPLETED) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitRequestOpenGLVisualizerCameraCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo); + + if (statusType) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitRequestContactPointInformation(sm); + + if (args.m_bodyUniqueIdA>=0) { + b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA); + } + if (args.m_bodyUniqueIdB>=0) { + b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB); + } + if (args.m_linkIndexA>=-1) { + b3SetContactFilterLinkA(command, args.m_linkIndexA); + } + if (args.m_linkIndexB >=-1) { + b3SetContactFilterLinkB(command, args.m_linkIndexB); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + b3GetContactPointInformation(sm, contactInfo); + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitClosestDistanceQuery(sm); + + b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA); + b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB); + b3SetClosestDistanceThreshold(command, distance); + + if (args.m_linkIndexA>=-1) { + b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA); + } + if (args.m_linkIndexB >=-1) { + b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + b3GetContactPointInformation(sm, contactInfo); + return true; + } + return false; +} + + +bool b3RobotSimulatorClientAPI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + // int statusType; + + command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + b3GetAABBOverlapResults(sm, overlapData); + + return true; +} + + +bool b3RobotSimulatorClientAPI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData) +{ + double daabbMin[3]; + double daabbMax[3]; + + daabbMin[0] = aabbMin.x; + daabbMin[1] = aabbMin.y; + daabbMin[2] = aabbMin.z; + + daabbMax[0] = aabbMax.x; + daabbMax[1] = aabbMax.y; + daabbMax[2] = aabbMax.z; + + return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData); +} + + + +bool b3RobotSimulatorClientAPI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (bodyUniqueId < 0) { + b3Warning("Invalid bodyUniqueId"); + return false; + } + + if (linkIndex < -1) { + b3Warning("Invalid linkIndex"); + return false; + } + + if (aabbMin == NULL || aabbMax == NULL) { + b3Warning("Output AABB matrix is NULL"); + return false; + } + + command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) { + return false; + } + if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax) +{ + double daabbMin[3]; + double daabbMax[3]; + + bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]); + + aabbMin.x = (float)daabbMin[0]; + aabbMin.y = (float)daabbMin[1]; + aabbMin.z = (float)daabbMin[2]; + + aabbMax.x = (float)daabbMax[0]; + aabbMax.y = (float)daabbMax[1]; + aabbMax.z = (float)daabbMax[2]; + + return status; +} + + +int b3RobotSimulatorClientAPI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int shapeIndex = -1; + + command = b3CreateCollisionShapeCommandInit(sm); + + if (shapeType==GEOM_SPHERE && args.m_radius>0) { + shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius); + } + if (shapeType==GEOM_BOX) { + double halfExtents[3]; + scalarToDouble3(args.m_halfExtents, halfExtents); + shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents); + } + if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) { + shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height); + } + if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) { + shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height); + } + if (shapeType==GEOM_MESH && args.m_fileName) { + double meshScale[3]; + scalarToDouble3(args.m_meshScale, meshScale); + shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale); + } + if (shapeType==GEOM_PLANE) { + double planeConstant=0; + double planeNormal[3]; + scalarToDouble3(args.m_planeNormal, planeNormal); + shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant); + } + if (shapeIndex>=0 && args.m_flags) { + b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) { + int uid = b3GetStatusCollisionShapeUniqueId(statusHandle); + return uid; + } + return -1; +} + +int b3RobotSimulatorClientAPI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType, baseIndex; + + double doubleBasePosition[3]; + double doubleBaseInertialFramePosition[3]; + scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition); + scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition); + + double doubleBaseOrientation[4]; + double doubleBaseInertialFrameOrientation[4]; + scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation); + scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation); + + command = b3CreateMultiBodyCommandInit(sm); + + baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex, + doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation); + + for (int i = 0; i < args.m_numLinks; i++) { + double linkMass = args.m_linkMasses[i]; + int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i]; + int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i]; + b3Vector3 linkPosition = args.m_linkPositions[i]; + b3Quaternion linkOrientation = args.m_linkOrientations[i]; + b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i]; + b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i]; + int linkParentIndex = args.m_linkParentIndices[i]; + int linkJointType = args.m_linkJointTypes[i]; + b3Vector3 linkJointAxis = args.m_linkJointAxes[i]; + + double doubleLinkPosition[3]; + double doubleLinkInertialFramePosition[3]; + double doubleLinkJointAxis[3]; + scalarToDouble3(linkPosition.m_floats, doubleLinkPosition); + scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition); + scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis); + + double doubleLinkOrientation[4]; + double doubleLinkInertialFrameOrientation[4]; + scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation); + scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation); + + b3CreateMultiBodyLink(command, + linkMass, + linkCollisionShapeIndex, + linkVisualShapeIndex, + doubleLinkPosition, + doubleLinkOrientation, + doubleLinkInertialFramePosition, + doubleLinkInertialFrameOrientation, + linkParentIndex, + linkJointType, + doubleLinkJointAxis + ); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) { + int uid = b3GetStatusBodyIndex(statusHandle); + return uid; + } + return -1; +} + +int b3RobotSimulatorClientAPI::getNumConstraints() const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return -1; + } + return b3GetNumUserConstraints(m_data->m_physicsClientHandle); +} + +int b3RobotSimulatorClientAPI::getConstraintUniqueId(int serialIndex) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return -1; + } + int userConstraintId = -1; + userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex); + return userConstraintId; +} \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index d2bee5338..37e9c0e9a 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -19,19 +19,19 @@ struct b3RobotSimulatorLoadUrdfFileArgs b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) : m_startPosition(startPos), - m_startOrientation(startOrn), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) + m_startOrientation(startOrn), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) { } b3RobotSimulatorLoadUrdfFileArgs() : m_startPosition(b3MakeVector3(0, 0, 0)), - m_startOrientation(b3Quaternion(0, 0, 0, 1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) + m_startOrientation(b3Quaternion(0, 0, 0, 1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) { } }; @@ -43,7 +43,7 @@ struct b3RobotSimulatorLoadSdfFileArgs b3RobotSimulatorLoadSdfFileArgs() : m_forceOverrideFixedBase(false), - m_useMultiBody(true) + m_useMultiBody(true) { } }; @@ -70,11 +70,11 @@ struct b3RobotSimulatorJointMotorArgs b3RobotSimulatorJointMotorArgs(int controlMode) : m_controlMode(controlMode), - m_targetPosition(0), - m_kp(0.1), - m_targetVelocity(0), - m_kd(0.9), - m_maxTorqueValue(1000) + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.9), + m_maxTorqueValue(1000) { } }; @@ -104,8 +104,8 @@ struct b3RobotSimulatorInverseKinematicArgs b3RobotSimulatorInverseKinematicArgs() : m_bodyUniqueId(-1), - m_endEffectorLinkIndex(-1), - m_flags(0) + m_endEffectorLinkIndex(-1), + m_flags(0) { m_endEffectorTargetPosition[0] = 0; m_endEffectorTargetPosition[1] = 0; @@ -126,13 +126,272 @@ struct b3RobotSimulatorInverseKinematicsResults struct b3JointStates2 { - int m_bodyUniqueId; - int m_numDegreeOfFreedomQ; - int m_numDegreeOfFreedomU; - b3Transform m_rootLocalInertialFrame; - b3AlignedObjectArray m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray m_jointReactionForces; + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray m_jointReactionForces; +}; + + +struct b3RobotSimulatorJointMotorArrayArgs +{ + int m_controlMode; + int m_numControlledDofs; + + int *m_jointIndices; + + double *m_targetPositions; + double *m_kps; + + double *m_targetVelocities; + double *m_kds; + + double *m_forces; + + b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) + : m_controlMode(controlMode), m_numControlledDofs(numControlledDofs) + { + } +}; + +struct b3RobotSimulatorGetCameraImageArgs +{ + int m_width; + int m_height; + float *m_viewMatrix; + float *m_projectionMatrix; + float *m_lightDirection; + float *m_lightColor; + float m_lightDistance; + int m_hasShadow; + float m_lightAmbientCoeff; + float m_lightDiffuseCoeff; + float m_lightSpecularCoeff; + int m_renderer; + + b3RobotSimulatorGetCameraImageArgs(int width, int height) + : m_width(width), + m_height(height), + m_viewMatrix(NULL), + m_projectionMatrix(NULL), + m_lightDirection(NULL), + m_lightColor(NULL), + m_lightDistance(-1), + m_hasShadow(-1), + m_lightAmbientCoeff(-1), + m_lightDiffuseCoeff(-1), + m_lightSpecularCoeff(-1), + m_renderer(-1) + { + } +}; + +struct b3RobotSimulatorSetPhysicsEngineParameters +{ + double m_fixedTimeStep; + int m_numSolverIterations; + int m_useSplitImpulse; + double m_splitImpulsePenetrationThreshold; + int m_numSubSteps; + int m_collisionFilterMode; + double m_contactBreakingThreshold; + int m_maxNumCmdPer1ms; + int m_enableFileCaching; + double m_restitutionVelocityThreshold; + double m_erp; + double m_contactERP; + double m_frictionERP; + + b3RobotSimulatorSetPhysicsEngineParameters() + : m_fixedTimeStep(-1), + m_numSolverIterations(-1), + m_useSplitImpulse(-1), + m_splitImpulsePenetrationThreshold(-1), + m_numSubSteps(-1), + m_collisionFilterMode(-1), + m_contactBreakingThreshold(-1), + m_maxNumCmdPer1ms(-1), + m_enableFileCaching(-1), + m_restitutionVelocityThreshold(-1), + m_erp(-1), + m_contactERP(-1), + m_frictionERP(-1) + {} +}; + +struct b3RobotSimulatorChangeDynamicsArgs +{ + double m_mass; + double m_lateralFriction; + double m_spinningFriction; + double m_rollingFriction; + double m_restitution; + double m_linearDamping; + double m_angularDamping; + double m_contactStiffness; + double m_contactDamping; + int m_frictionAnchor; + + b3RobotSimulatorChangeDynamicsArgs() + : m_mass(-1), + m_lateralFriction(-1), + m_spinningFriction(-1), + m_rollingFriction(-1), + m_restitution(-1), + m_linearDamping(-1), + m_angularDamping(-1), + m_contactStiffness(-1), + m_contactDamping(-1), + m_frictionAnchor(-1) + {} +}; + +struct b3RobotSimulatorAddUserDebugLineArgs +{ + double m_colorRGB[3]; + double m_lineWidth; + double m_lifeTime; + int m_parentObjectUniqueId; + int m_parentLinkIndex; + + b3RobotSimulatorAddUserDebugLineArgs() + : + m_lineWidth(1), + m_lifeTime(0), + m_parentObjectUniqueId(-1), + m_parentLinkIndex(-1) + { + m_colorRGB[0] = 1; + m_colorRGB[1] = 1; + m_colorRGB[2] = 1; + } +}; + +enum b3AddUserDebugTextFlags { + DEBUG_TEXT_HAS_ORIENTATION = 1 +}; + +struct b3RobotSimulatorAddUserDebugTextArgs +{ + double m_colorRGB[3]; + double m_size; + double m_lifeTime; + double m_textOrientation[4]; + int m_parentObjectUniqueId; + int m_parentLinkIndex; + int m_flags; + + b3RobotSimulatorAddUserDebugTextArgs() + : m_size(1), + m_lifeTime(0), + m_parentObjectUniqueId(-1), + m_parentLinkIndex(-1), + m_flags(0) + { + m_colorRGB[0] = 1; + m_colorRGB[1] = 1; + m_colorRGB[2] = 1; + + m_textOrientation[0] = 0; + m_textOrientation[1] = 0; + m_textOrientation[2] = 0; + m_textOrientation[3] = 1; + + } +}; + +struct b3RobotSimulatorGetContactPointsArgs +{ + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + + b3RobotSimulatorGetContactPointsArgs() + : m_bodyUniqueIdA(-1), + m_bodyUniqueIdB(-1), + m_linkIndexA(-2), + m_linkIndexB(-2) + {} +}; + +struct b3RobotSimulatorCreateCollisionShapeArgs +{ + int m_shapeType; + double m_radius; + b3Vector3 m_halfExtents; + double m_height; + char* m_fileName; + b3Vector3 m_meshScale; + b3Vector3 m_planeNormal; + int m_flags; + b3RobotSimulatorCreateCollisionShapeArgs() + : m_shapeType(-1), + m_radius(0.5), + m_height(1), + m_fileName(NULL), + m_flags(0) + { + m_halfExtents.m_floats[0] = 1; + m_halfExtents.m_floats[1] = 1; + m_halfExtents.m_floats[2] = 1; + + m_meshScale.m_floats[0] = 1; + m_meshScale.m_floats[1] = 1; + m_meshScale.m_floats[2] = 1; + + m_planeNormal.m_floats[0] = 0; + m_planeNormal.m_floats[1] = 0; + m_planeNormal.m_floats[2] = 1; + } + +}; + +struct b3RobotSimulatorCreateMultiBodyArgs +{ + double m_baseMass; + int m_baseCollisionShapeIndex; + int m_baseVisualShapeIndex; + b3Vector3 m_basePosition; + b3Quaternion m_baseOrientation; + b3Vector3 m_baseInertialFramePosition; + b3Quaternion m_baseInertialFrameOrientation; + + int m_numLinks; + double *m_linkMasses; + int *m_linkCollisionShapeIndices; + int *m_linkVisualShapeIndices; + b3Vector3 *m_linkPositions; + b3Quaternion *m_linkOrientations; + b3Vector3 *m_linkInertialFramePositions; + b3Quaternion *m_linkInertialFrameOrientations; + int *m_linkParentIndices; + int *m_linkJointTypes; + b3Vector3 *m_linkJointAxes; + + int m_useMaximalCoordinates; + + b3RobotSimulatorCreateMultiBodyArgs() + : m_numLinks(0), m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_useMaximalCoordinates(0), + m_linkMasses(NULL), + m_linkCollisionShapeIndices(NULL), + m_linkVisualShapeIndices(NULL), + m_linkPositions(NULL), + m_linkOrientations(NULL), + m_linkInertialFramePositions(NULL), + m_linkInertialFrameOrientations(NULL), + m_linkParentIndices(NULL), + m_linkJointTypes(NULL), + m_linkJointAxes(NULL) + { + m_basePosition.setValue(0,0,0); + m_baseOrientation.setValue(0,0,0,1); + m_baseInertialFramePosition.setValue(0,0,0); + m_baseInertialFrameOrientation.setValue(0,0,0,1); + } }; ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet @@ -194,6 +453,10 @@ public: void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); + bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs, + int *jointIndices, double *targetVelocities, double *targetPositions, + double *forces, double *kps, double *kds); + void stepSimulation(); bool canSubmitCommand() const; @@ -224,8 +487,77 @@ public: void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); - void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); + void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); + // JFC: added these 24 methods + + void getMouseEvents(b3MouseEventsData* mouseEventsData); + + bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeInverseKinematics, b3LinkState* linkState); + + bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData); + + bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput); + + int getNumBodies() const; + + int getBodyUniqueId(int bodyId) const; + + bool removeBody(int bodyUniqueId); + + bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo); + + bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args); + + int addUserDebugParameter(char *paramName, double rangeMin, double rangeMax, double startValue); + + double readUserDebugParameter(int itemUniqueId); + + bool removeUserDebugItem(int itemUniqueId); + + int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + + int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + + int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + + int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + + bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); + + bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args); + + bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags); + + bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags); + + bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags); + + bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags); + + bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable); + + bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo); + + bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo); + + bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo); + + bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData); + + bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData); + + bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax); + + bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax); + + int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); + + int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args); + + int getNumConstraints() const; + + int getConstraintUniqueId(int serialIndex); //////////////// INTERNAL From 433f8449f4725632b3cedecff3e560e80a06e9c5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Feb 2018 18:33:29 -0800 Subject: [PATCH 3/7] refactor b3RobotSimulatorClientAPI into a main part without GUI dependencies (no OpenGL, gwen, glew etc) so that App_RobotSimulator_NoGUI can link against BulletRobotics and App_RobotSimulator links against BulletRobotics and some extra files. --- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/premake4.lua | 2 + examples/RobotSimulator/CMakeLists.txt | 178 +- examples/RobotSimulator/MinitaurSetup.cpp | 8 +- examples/RobotSimulator/MinitaurSetup.h | 6 +- .../RobotSimulator/RobotSimulatorMain.cpp | 24 +- .../b3RobotSimulatorClientAPI.cpp | 1890 +--------------- .../b3RobotSimulatorClientAPI.h | 568 +---- .../b3RobotSimulatorClientAPI_InternalData.h | 18 + .../b3RobotSimulatorClientAPI_NoGUI.cpp | 1997 +++++++++++++++++ .../b3RobotSimulatorClientAPI_NoGUI.h | 572 +++++ examples/RobotSimulator/premake4.lua | 4 + examples/SharedMemory/PhysicsClientC_API.h | 1 - examples/TwoJoint/TwoJointMain.cpp | 1 + 14 files changed, 2698 insertions(+), 2573 deletions(-) create mode 100644 examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h create mode 100644 examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp create mode 100644 examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 2554ac4fa..aec69df37 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -175,6 +175,8 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/b3PluginManager.cpp ../RobotSimulator/b3RobotSimulatorClientAPI.cpp ../RobotSimulator/b3RobotSimulatorClientAPI.h + ../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp + ../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h ../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.h ../InverseDynamics/InverseDynamicsExample.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 56789385c..65e38ece8 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -122,6 +122,8 @@ project "App_BulletExampleBrowser" "../InverseDynamics/InverseDynamicsExample.h", "../RobotSimulator/b3RobotSimulatorClientAPI.cpp", "../RobotSimulator/b3RobotSimulatorClientAPI.h", + "../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp", + "../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", "../ExtendedTutorials/*", diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index d0bb13eaf..cb06becbd 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -1,4 +1,3 @@ - INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/examples @@ -9,77 +8,20 @@ INCLUDE_DIRECTORIES( SET(RobotSimulator_SRCS - RobotSimulatorMain.cpp - b3RobotSimulatorClientAPI.cpp - b3RobotSimulatorClientAPI.h - MinitaurSetup.cpp - MinitaurSetup.h - ../../examples/SharedMemory/IKTrajectoryHelper.cpp - ../../examples/SharedMemory/IKTrajectoryHelper.h - ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp - ../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp - ../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp - ../../examples/OpenGLWindow/SimpleCamera.cpp - ../../examples/OpenGLWindow/SimpleCamera.h - ../../examples/TinyRenderer/geometry.cpp - ../../examples/TinyRenderer/model.cpp - ../../examples/TinyRenderer/tgaimage.cpp - ../../examples/TinyRenderer/our_gl.cpp - ../../examples/TinyRenderer/TinyRenderer.cpp - ../../examples/SharedMemory/InProcessMemory.cpp - ../../examples/SharedMemory/PhysicsClient.cpp - ../../examples/SharedMemory/PhysicsClient.h - ../../examples/SharedMemory/PhysicsServer.cpp - ../../examples/SharedMemory/PhysicsServer.h - ../../examples/SharedMemory/PhysicsServerExample.cpp - ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp - ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.h - ../../examples/SharedMemory/PhysicsDirect.cpp - ../../examples/SharedMemory/PhysicsDirect.h - ../../examples/SharedMemory/PhysicsDirectC_API.cpp - ../../examples/SharedMemory/PhysicsDirectC_API.h - ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp - ../../examples/SharedMemory/PhysicsServerCommandProcessor.h - ../../examples/SharedMemory/b3PluginManager.cpp - - ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory.h - ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h - ../../examples/SharedMemory/PhysicsClientC_API.cpp - ../../examples/SharedMemory/PhysicsClientC_API.h - ../../examples/SharedMemory/Win32SharedMemory.cpp - ../../examples/SharedMemory/Win32SharedMemory.h - ../../examples/SharedMemory/PosixSharedMemory.cpp - ../../examples/SharedMemory/PosixSharedMemory.h - ../../examples/Utils/b3ResourcePath.cpp - ../../examples/Utils/b3ResourcePath.h - ../../examples/Utils/RobotLoggingUtil.cpp - ../../examples/Utils/RobotLoggingUtil.h - ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h - ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp - ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp - ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp - ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp - ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp - ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp - ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp - ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp - ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp - ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp - ../../examples/MultiThreading/b3PosixThreadSupport.cpp - ../../examples/MultiThreading/b3Win32ThreadSupport.cpp - ../../examples/MultiThreading/b3ThreadSupportInterface.cpp - -) +RobotSimulatorMain.cpp + b3RobotSimulatorClientAPI.cpp + b3RobotSimulatorClientAPI.h + b3RobotSimulatorClientAPI_NoGUI.cpp + b3RobotSimulatorClientAPI_NoGUI.h + MinitaurSetup.cpp + MinitaurSetup.h + ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp + ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +) + + IF(BUILD_CLSOCKET) ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET) @@ -89,54 +31,9 @@ IF(WIN32) LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) - IF(BUILD_ENET) - ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET) - ENDIF(BUILD_ENET) - IF(BUILD_CLSOCKET) - ADD_DEFINITIONS(-DWIN32) - ENDIF(BUILD_CLSOCKET) - -ELSE(WIN32) - IF(BUILD_ENET) - ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET) - ENDIF(BUILD_ENET) - - IF(BUILD_CLSOCKET) - ADD_DEFINITIONS(${OSDEF}) - ENDIF(BUILD_CLSOCKET) ENDIF(WIN32) -IF(BUILD_ENET) - set(RobotSimulator_SRCS ${RobotSimulator_SRCS} - ../../examples/SharedMemory/PhysicsClientUDP.cpp - ../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp - ../../examples/SharedMemory/PhysicsClientUDP.h - ../../examples/SharedMemory/PhysicsClientUDP_C_API.h - ../../examples/ThirdPartyLibs/enet/win32.c - ../../examples/ThirdPartyLibs/enet/unix.c - ../../examples/ThirdPartyLibs/enet/callbacks.c - ../../examples/ThirdPartyLibs/enet/compress.c - ../../examples/ThirdPartyLibs/enet/host.c - ../../examples/ThirdPartyLibs/enet/list.c - ../../examples/ThirdPartyLibs/enet/packet.c - ../../examples/ThirdPartyLibs/enet/peer.c - ../../examples/ThirdPartyLibs/enet/protocol.c - ) -ENDIF(BUILD_ENET) - -IF(BUILD_CLSOCKET) - set(RobotSimulator_SRCS ${RobotSimulator_SRCS} - ../../examples/SharedMemory/PhysicsClientTCP.cpp - ../../examples/SharedMemory/PhysicsClientTCP.h - ../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp - ../../examples/SharedMemory/PhysicsClientTCP_C_API.h - ../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp - ../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp - ../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp - ) -ENDIF() - #some code to support OpenGL and Glew cross platform IF (WIN32) @@ -168,6 +65,8 @@ ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS}) SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION}) SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d") +SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES COMPILE_DEFINITIONS "B3_USE_ROBOTSIM_GUI") + IF(WIN32) @@ -176,8 +75,51 @@ IF(WIN32) ENDIF(BUILD_ENET OR BUILD_CLSOCKET) ENDIF(WIN32) +TARGET_LINK_LIBRARIES(App_RobotSimulator BulletRobotics BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common) + + + + + + + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src + ${BULLET_PHYSICS_SOURCE_DIR}/examples + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src + ) + +SET(RobotSimulator_NoGUI_SRCS + RobotSimulatorMain.cpp + b3RobotSimulatorClientAPI_NoGUI.cpp + b3RobotSimulatorClientAPI_NoGUI.h + MinitaurSetup.cpp + MinitaurSetup.h +) + +ADD_EXECUTABLE(App_RobotSimulator_NoGUI ${RobotSimulator_NoGUI_SRCS}) + +SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES DEBUG_POSTFIX "_d") + + +IF(WIN32) + IF(BUILD_ENET OR BUILD_CLSOCKET) + TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI ws2_32 ) + ENDIF(BUILD_ENET OR BUILD_CLSOCKET) +ENDIF(WIN32) + +TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath Bullet3Common) + + + + + + + -TARGET_LINK_LIBRARIES(App_RobotSimulator BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common) diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index ec4a174e4..16d102072 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -1,5 +1,5 @@ #include "MinitaurSetup.h" -#include "b3RobotSimulatorClientAPI.h" +#include "b3RobotSimulatorClientAPI_NoGUI.h" #include "Bullet3Common/b3HashMap.h" @@ -27,7 +27,7 @@ MinitaurSetup::~MinitaurSetup() delete m_data; } -void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd) +void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); controlArgs.m_maxTorqueValue = maxTorque; @@ -158,7 +158,7 @@ static const char* minitaurURDF="quadruped/minitaur_rainbow_dash_v1.urdf"; #endif -void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) +void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim) { //release all motors int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); @@ -255,7 +255,7 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) } -int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) +int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) { b3RobotSimulatorLoadUrdfFileArgs args; diff --git a/examples/RobotSimulator/MinitaurSetup.h b/examples/RobotSimulator/MinitaurSetup.h index a054ade92..6d9f8c211 100644 --- a/examples/RobotSimulator/MinitaurSetup.h +++ b/examples/RobotSimulator/MinitaurSetup.h @@ -6,15 +6,15 @@ class MinitaurSetup { struct MinitaurSetupInternalData* m_data; - void resetPose(class b3RobotSimulatorClientAPI* sim); + void resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim); public: MinitaurSetup(); virtual ~MinitaurSetup(); - int setupMinitaur(class b3RobotSimulatorClientAPI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1)); + int setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1)); - void setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9); + void setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9); }; #endif //MINITAUR_SIMULATION_SETUP_H diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index c3711f20f..2a6b90584 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -1,5 +1,11 @@ -#include "b3RobotSimulatorClientAPI.h" +#ifdef B3_USE_ROBOTSIM_GUI + #include "b3RobotSimulatorClientAPI.h" +#else + #include "b3RobotSimulatorClientAPI_NoGUI.h" +#endif + + #include "../Utils/b3Clock.h" #include @@ -7,11 +13,23 @@ #include #define ASSERT_EQ(a,b) assert((a)==(b)); #include "MinitaurSetup.h" + + + int main(int argc, char* argv[]) { +#ifdef B3_USE_ROBOTSIM_GUI b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI(); - - sim->connect(eCONNECT_GUI); + bool isConnected = sim->connect(eCONNECT_GUI); +#else + b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI(); + bool isConnected = sim->connect(eCONNECT_DIRECT); +#endif + if (!isConnected) + { + printf("Cannot connect\n"); + return -1; + } //Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example: //sim->connect(eCONNECT_UDP, "localhost", 1234); sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 9bc1f91c0..f22e4c0aa 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -1,9 +1,7 @@ #include "b3RobotSimulatorClientAPI.h" -//#include "SharedMemoryCommands.h" - #include "../SharedMemory/PhysicsClientC_API.h" - +#include "b3RobotSimulatorClientAPI_InternalData.h"" #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //PHYSICS_UDP @@ -16,52 +14,20 @@ #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" - #include "../SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" -static void scalarToDouble3(b3Scalar a[3], double b[3]) -{ - for (int i = 0; i < 3; i++) - { - b[i] = a[i]; - } -} - -static void scalarToDouble4(b3Scalar a[4], double b[4]) -{ - for (int i = 0; i < 4; i++) - { - b[i] = a[i]; - } -} - -struct b3RobotSimulatorClientAPI_InternalData -{ - b3PhysicsClientHandle m_physicsClientHandle; - struct GUIHelperInterface* m_guiHelper; - - b3RobotSimulatorClientAPI_InternalData() - : m_physicsClientHandle(0), - m_guiHelper(0) - { - } -}; b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI() { - m_data = new b3RobotSimulatorClientAPI_InternalData(); + } b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() { - delete m_data; } -void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper) -{ - m_data->m_guiHelper = guiHelper; -} + void b3RobotSimulatorClientAPI::renderScene() { @@ -224,1853 +190,3 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i } return false; } - -bool b3RobotSimulatorClientAPI::isConnected() const -{ - return (m_data->m_physicsClientHandle != 0); -} - -void b3RobotSimulatorClientAPI::setTimeOut(double timeOutInSec) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec); - -} - - -void b3RobotSimulatorClientAPI::disconnect() -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3DisconnectSharedMemory(m_data->m_physicsClientHandle); - m_data->m_physicsClientHandle = 0; -} - -void b3RobotSimulatorClientAPI::syncBodies() -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - statusType = b3GetStatusType(statusHandle); -} - -void b3RobotSimulatorClientAPI::resetSimulation() -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus( - m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); -} - -bool b3RobotSimulatorClientAPI::canSubmitCommand() const -{ - if (!isConnected()) - { - return false; - } - return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); -} - -void b3RobotSimulatorClientAPI::stepSimulation() -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); - statusType = b3GetStatusType(statusHandle); - //b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED); - } -} - -void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - // b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); -} - -b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) -{ - b3Quaternion q; - q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); - return q; -} - -b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) -{ - b3Scalar roll,pitch,yaw; - quat.getEulerZYX(yaw,pitch,roll); - b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); - return rpy2; -} - -int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) -{ - int robotUniqueId = -1; - - if (!isConnected()) - { - b3Warning("Not connected"); - return robotUniqueId; - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - - //setting the initial position, orientation and other arguments are optional - - b3LoadUrdfCommandSetFlags(command,args.m_flags); - - b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); - b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); - if (args.m_forceOverrideFixedBase) - { - b3LoadUrdfCommandSetUseFixedBase(command, true); - } - b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - statusType = b3GetStatusType(statusHandle); - - b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); - if (statusType == CMD_URDF_LOADING_COMPLETED) - { - robotUniqueId = b3GetStatusBodyIndex(statusHandle); - } - return robotUniqueId; -} - -bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command; - - command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_MJCF_LOADING_COMPLETED) - { - b3Warning("Couldn't load .mjcf file."); - return false; - } - int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); - if (numBodies) - { - results.m_uniqueObjectIds.resize(numBodies); - int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); - } - - return true; -} - -bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command; - - command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_BULLET_LOADING_COMPLETED) - { - return false; - } - int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); - if (numBodies) - { - results.m_uniqueObjectIds.resize(numBodies); - int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); - } - - return true; -} - -bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - bool statusOk = false; - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); - if (statusType == CMD_SDF_LOADING_COMPLETED) - { - int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); - if (numBodies) - { - results.m_uniqueObjectIds.resize(numBodies); - int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); - } - statusOk = true; - } - - return statusOk; -} - -bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); - return (result != 0); -} - -bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); - - const int status_type = b3GetStatusType(status_handle); - const double* actualStateQ; - - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - return false; - } - - b3GetStatusActualState( - status_handle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, &actualStateQ, - 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); - - basePosition[0] = actualStateQ[0]; - basePosition[1] = actualStateQ[1]; - basePosition[2] = actualStateQ[2]; - - baseOrientation[0] = actualStateQ[3]; - baseOrientation[1] = actualStateQ[4]; - baseOrientation[2] = actualStateQ[5]; - baseOrientation[3] = actualStateQ[6]; - return true; -} - -bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle commandHandle; - - commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - - b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); - b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], - baseOrientation[2], baseOrientation[3]); - - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - - return true; -} - -bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - const int status_type = b3GetStatusType(statusHandle); - const double* actualStateQdot; - - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - return false; - } - - b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, 0, - &actualStateQdot, 0 /* joint_reaction_forces */); - - baseLinearVelocity[0] = actualStateQdot[0]; - baseLinearVelocity[1] = actualStateQdot[1]; - baseLinearVelocity[2] = actualStateQdot[2]; - - baseAngularVelocity[0] = actualStateQdot[3]; - baseAngularVelocity[1] = actualStateQdot[4]; - baseAngularVelocity[2] = actualStateQdot[5]; - return true; -} - -bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - - commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - - b3Vector3DoubleData linVelDouble; - linearVelocity.serializeDouble(linVelDouble); - b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); - - b3Vector3DoubleData angVelDouble; - angularVelocity.serializeDouble(angVelDouble); - b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); - - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - return true; -} - -void b3RobotSimulatorClientAPI::setInternalSimFlags(int flags) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - { - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetInternalSimFlags(command, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - } -} - - -void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - - int ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); - - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); -} - -int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); -} - -bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); -} - -int b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return -1; - } - b3SharedMemoryStatusHandle statusHandle; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); - if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); - int statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_CONSTRAINT_COMPLETED) - { - int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); - return userConstraintUid; - } - } - return -1; -} - -int b3RobotSimulatorClientAPI::changeConstraint(int constraintId, b3JointInfo* jointInfo) -{ - - if (!isConnected()) - { - b3Warning("Not connected"); - return -1; - } - b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); - - if (jointInfo->m_flags & eJointChangeMaxForce) - { - b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce); - } - - if (jointInfo->m_flags & eJointChangeChildFramePosition) - { - b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]); - } - if (jointInfo->m_flags & eJointChangeChildFrameOrientation) - { - b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]); - } - - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - int statusType = b3GetStatusType(statusHandle); - return statusType; -} - -void b3RobotSimulatorClientAPI::removeConstraint(int constraintId) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - int statusType = b3GetStatusType(statusHandle); -} - - -bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - int statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) - { - return true; - } - } - return false; -} - -bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& state) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (statusHandle) - { - // double rootInertialFrame[7]; - const double* rootLocalInertialFrame; - const double* actualStateQ; - const double* actualStateQdot; - const double* jointReactionForces; - - int stat = b3GetStatusActualState(statusHandle, - &state.m_bodyUniqueId, - &state.m_numDegreeOfFreedomQ, - &state.m_numDegreeOfFreedomU, - &rootLocalInertialFrame, - &actualStateQ, - &actualStateQdot, - &jointReactionForces); - if (stat) - { - state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); - state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); - - for (int i=0;im_physicsClientHandle, bodyUniqueId); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - return false; - } - - commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - - b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, - targetValue); - - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - return false; -} - -void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryStatusHandle statusHandle; - switch (args.m_controlMode) - { - case CONTROL_MODE_VELOCITY: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - b3JointControlSetKd(command, uIndex, args.m_kd); - b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); - b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - int qIndex = jointInfo.m_qIndex; - - b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition); - b3JointControlSetKp(command, uIndex, args.m_kp); - b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); - b3JointControlSetKd(command, uIndex, args.m_kd); - b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - break; - } - case CONTROL_MODE_TORQUE: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - break; - } - default: - { - b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl"); - } - } -} - -void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSolverIterations(command, numIterations); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); -} - -void b3RobotSimulatorClientAPI::setContactBreakingThreshold(double threshold) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetContactBreakingThreshold(command,threshold); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); -} - - -void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - int ret; - ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); -} - -void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSubSteps(command, numSubSteps); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); -} - -bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - b3Assert(args.m_endEffectorLinkIndex >= 0); - b3Assert(args.m_bodyUniqueId >= 0); - - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId); - if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) - { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } - else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) - { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation); - } - else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } - else - { - b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition); - } - - if (args.m_flags & B3_HAS_JOINT_DAMPING) - { - b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); - } - - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - int numPos = 0; - - bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0) != 0; - if (result && numPos) - { - results.m_calculatedJointPositions.resize(numPos); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0]) != 0; - } - return result; -} - -bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) - { - int dofCount; - b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian); - return true; - } - return false; -} - -bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) -{ - if (!isConnected()) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - - if (computeLinkVelocity) { - b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity); - } - - if (computeForwardKinematics) { - b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics); - } - - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - - if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); - return true; - } - return false; -} - -void b3RobotSimulatorClientAPI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); - b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); -} - -void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) -{ - vrEventsData->m_numControllerEvents = 0; - vrEventsData->m_controllerEvents = 0; - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); - b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); -} - -void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) -{ - keyboardEventsData->m_numKeyboardEvents = 0; - keyboardEventsData->m_keyboardEvents = 0; - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); -} - -int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return -1; - } - int loggingUniqueId = -1; - b3SharedMemoryCommandHandle commandHandle; - commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); - - b3StateLoggingStart(commandHandle, loggingType, fileName.c_str()); - - for (int i = 0; i < objectUniqueIds.size(); i++) - { - int objectUid = objectUniqueIds[i]; - b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid); - } - - if (maxLogDof > 0) - { - b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_STATE_LOGGING_START_COMPLETED) - { - loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); - } - return loggingUniqueId; -} - -void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); - b3StateLoggingStop(commandHandle, stateLoggerUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); -} - -void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); - if (commandHandle) - { - if ((cameraDistance >= 0)) - { - b3Vector3FloatData camTargetPos; - targetPos.serializeFloat(camTargetPos); - b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); - } - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - } -} - -void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str()); - if (durationInMicroSeconds>=0) - { - b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds); - } - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); -} - -void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSoftBodySetScale(command, scale); - b3LoadSoftBodySetMass(command, mass); - b3LoadSoftBodySetCollisionMargin(command, collisionMargin); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); -} - -void b3RobotSimulatorClientAPI::getMouseEvents(b3MouseEventsData* mouseEventsData) -{ - mouseEventsData->m_numMouseEvents = 0; - mouseEventsData->m_mouseEvents = 0; - if (!isConnected()) { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData); -} - - -bool b3RobotSimulatorClientAPI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, - double *jointAccelerations, double *jointForcesOutput) -{ - if (!isConnected()) { - b3Warning("Not connected"); - return false; - } - - int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, - jointVelocities, jointAccelerations); - - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { - int bodyUniqueId; - int dofCount; - - b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0); - - if (dofCount) { - b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput); - return true; - } - } - return false; -} - -int b3RobotSimulatorClientAPI::getNumBodies() const -{ - if (!isConnected()) { - b3Warning("Not connected"); - return false; - } - return b3GetNumBodies(m_data->m_physicsClientHandle); -} - -int b3RobotSimulatorClientAPI::getBodyUniqueId(int bodyId) const -{ - if (!isConnected()) { - b3Warning("Not connected"); - return false; - } - return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId); -} - -bool b3RobotSimulatorClientAPI::removeBody(int bodyUniqueId) -{ - if (!isConnected()) { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId)); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_REMOVE_BODY_COMPLETED) { - return true; - } else { - b3Warning("getDynamicsInfo did not complete"); - return false; - } - } - b3Warning("removeBody could not submit command"); - return false; -} - -bool b3RobotSimulatorClientAPI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) { - if (!isConnected()) { - b3Warning("Not connected"); - return false; - } - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle; - b3SharedMemoryStatusHandle status_handle; - // struct b3DynamicsInfo info; - - if (bodyUniqueId < 0) { - b3Warning("getDynamicsInfo failed; invalid bodyUniqueId"); - return false; - } - if (linkIndex < -1) { - b3Warning("getDynamicsInfo failed; invalid linkIndex"); - return false; - } - - if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { - cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex); - status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); - status_type = b3GetStatusType(status_handle); - if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) { - return true; - } else { - b3Warning("getDynamicsInfo did not complete"); - return false; - } - } - b3Warning("getDynamicsInfo could not submit command"); - return false; -} - - -bool b3RobotSimulatorClientAPI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return false; - } - b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (args.m_mass >= 0) { - b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass); - } - - if (args.m_lateralFriction >= 0) { - b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction); - } - - if (args.m_spinningFriction>=0) { - b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction); - } - - if (args.m_rollingFriction>=0) { - b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction); - } - - if (args.m_linearDamping>=0) { - b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping); - } - - if (args.m_angularDamping>=0) { - b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping); - } - - if (args.m_restitution>=0) { - b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution); - } - - if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) { - b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping); - } - - if (args.m_frictionAnchor>=0) { - b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor); - } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - return true; -} - -int b3RobotSimulatorClientAPI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) { - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return -1; - } - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - return debugItemUniqueId; - } - b3Warning("addUserDebugParameter failed."); - return -1; -} - - -double b3RobotSimulatorClientAPI::readUserDebugParameter(int itemUniqueId) { - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return 0; - } - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) { - double paramValue = 0.f; - int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue); - if (ok) { - return paramValue; - } - } - b3Warning("readUserDebugParameter failed."); - return 0; -} - - -bool b3RobotSimulatorClientAPI::removeUserDebugItem(int itemUniqueId) { - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return false; - } - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - return true; -} - - -int b3RobotSimulatorClientAPI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return -1; - } - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime); - - if (args.m_parentObjectUniqueId>=0) { - b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); - } - - if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) { - b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - return debugItemUniqueId; - } - b3Warning("addUserDebugText3D failed."); - return -1; -} - -int b3RobotSimulatorClientAPI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) -{ - double dposXYZ[3]; - dposXYZ[0] = posXYZ.x; - dposXYZ[1] = posXYZ.y; - dposXYZ[2] = posXYZ.z; - - return addUserDebugText(text, &dposXYZ[0], args); -} - -int b3RobotSimulatorClientAPI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return -1; - } - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime); - - if (args.m_parentObjectUniqueId>=0) { - b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - return debugItemUniqueId; - } - b3Warning("addUserDebugLine failed."); - return -1; -} - -int b3RobotSimulatorClientAPI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) -{ - double dfromXYZ[3]; - double dtoXYZ[3]; - dfromXYZ[0] = fromXYZ.x; - dfromXYZ[1] = fromXYZ.y; - dfromXYZ[2] = fromXYZ.z; - - dtoXYZ[0] = toXYZ.x; - dtoXYZ[1] = toXYZ.y; - dtoXYZ[2] = toXYZ.z; - return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args); -} - - - -bool b3RobotSimulatorClientAPI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected to physics server."); - return false; - } - int numJoints = b3GetNumJoints(sm, bodyUniqueId); - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - // int statusType; - struct b3JointInfo info; - - commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode); - - for (int i=0;im_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (args.m_numSolverIterations >= 0) { - b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations); - } - - if (args.m_collisionFilterMode >= 0) { - b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode); - } - - if (args.m_numSubSteps >= 0) { - b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps); - } - - if (args.m_fixedTimeStep >= 0) { - b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep); - } - - if (args.m_useSplitImpulse >= 0) { - b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse); - } - - if (args.m_splitImpulsePenetrationThreshold >= 0) { - b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold); - } - - if (args.m_contactBreakingThreshold >= 0) { - b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold); - } - - if (args.m_maxNumCmdPer1ms >= -1) { - b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms); - } - - if (args.m_restitutionVelocityThreshold>=0) { - b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold); - } - - if (args.m_enableFileCaching>=0) { - b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching); - } - - if (args.m_erp>=0) { - b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp); - } - - if (args.m_contactERP>=0) { - b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP); - } - - if (args.m_frictionERP >=0) { - b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - return true; -} - - -bool b3RobotSimulatorClientAPI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - - command = b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - return true; -} - -bool b3RobotSimulatorClientAPI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags) -{ - double dforce[3]; - double dposition[3]; - - dforce[0] = force.x; - dforce[1] = force.y; - dforce[2] = force.z; - - dposition[0] = position.x; - dposition[1] = position.y; - dposition[2] = position.z; - - return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags); -} - - -bool b3RobotSimulatorClientAPI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - - command = b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - return true; -} - -bool b3RobotSimulatorClientAPI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags) -{ - double dtorque[3]; - - dtorque[0] = torque.x; - dtorque[1] = torque.y; - dtorque[2] = torque.z; - - return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags); -} - - -bool b3RobotSimulatorClientAPI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - int numJoints = b3GetNumJoints(sm, bodyUniqueId); - if ((jointIndex < 0) || (jointIndex >= numJoints)) { - b3Warning("Error: invalid jointIndex."); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - command = b3CreateSensorCommandInit(sm, bodyUniqueId); - b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CLIENT_COMMAND_COMPLETED) { - return true; - } - return false; -} - -bool b3RobotSimulatorClientAPI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - command = b3InitRequestOpenGLVisualizerCameraCommand(sm); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo); - - if (statusType) { - return true; - } - return false; -} - -bool b3RobotSimulatorClientAPI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - command = b3InitRequestContactPointInformation(sm); - - if (args.m_bodyUniqueIdA>=0) { - b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA); - } - if (args.m_bodyUniqueIdB>=0) { - b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB); - } - if (args.m_linkIndexA>=-1) { - b3SetContactFilterLinkA(command, args.m_linkIndexA); - } - if (args.m_linkIndexB >=-1) { - b3SetContactFilterLinkB(command, args.m_linkIndexB); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - b3GetContactPointInformation(sm, contactInfo); - return true; - } - return false; -} - -bool b3RobotSimulatorClientAPI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - command = b3InitClosestDistanceQuery(sm); - - b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA); - b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB); - b3SetClosestDistanceThreshold(command, distance); - - if (args.m_linkIndexA>=-1) { - b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA); - } - if (args.m_linkIndexB >=-1) { - b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - b3GetContactPointInformation(sm, contactInfo); - return true; - } - return false; -} - - -bool b3RobotSimulatorClientAPI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - // int statusType; - - command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - b3GetAABBOverlapResults(sm, overlapData); - - return true; -} - - -bool b3RobotSimulatorClientAPI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData) -{ - double daabbMin[3]; - double daabbMax[3]; - - daabbMin[0] = aabbMin.x; - daabbMin[1] = aabbMin.y; - daabbMin[2] = aabbMin.z; - - daabbMax[0] = aabbMax.x; - daabbMax[1] = aabbMax.y; - daabbMax[2] = aabbMax.z; - - return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData); -} - - - -bool b3RobotSimulatorClientAPI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - if (bodyUniqueId < 0) { - b3Warning("Invalid bodyUniqueId"); - return false; - } - - if (linkIndex < -1) { - b3Warning("Invalid linkIndex"); - return false; - } - - if (aabbMin == NULL || aabbMax == NULL) { - b3Warning("Output AABB matrix is NULL"); - return false; - } - - command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) { - return false; - } - if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) { - return true; - } - return false; -} - -bool b3RobotSimulatorClientAPI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax) -{ - double daabbMin[3]; - double daabbMax[3]; - - bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]); - - aabbMin.x = (float)daabbMin[0]; - aabbMin.y = (float)daabbMin[1]; - aabbMin.z = (float)daabbMin[2]; - - aabbMax.x = (float)daabbMax[0]; - aabbMax.y = (float)daabbMax[1]; - aabbMax.z = (float)daabbMax[2]; - - return status; -} - - -int b3RobotSimulatorClientAPI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int shapeIndex = -1; - - command = b3CreateCollisionShapeCommandInit(sm); - - if (shapeType==GEOM_SPHERE && args.m_radius>0) { - shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius); - } - if (shapeType==GEOM_BOX) { - double halfExtents[3]; - scalarToDouble3(args.m_halfExtents, halfExtents); - shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents); - } - if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) { - shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height); - } - if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) { - shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height); - } - if (shapeType==GEOM_MESH && args.m_fileName) { - double meshScale[3]; - scalarToDouble3(args.m_meshScale, meshScale); - shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale); - } - if (shapeType==GEOM_PLANE) { - double planeConstant=0; - double planeNormal[3]; - scalarToDouble3(args.m_planeNormal, planeNormal); - shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant); - } - if (shapeIndex>=0 && args.m_flags) { - b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) { - int uid = b3GetStatusCollisionShapeUniqueId(statusHandle); - return uid; - } - return -1; -} - -int b3RobotSimulatorClientAPI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args) -{ - b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { - b3Warning("Not connected"); - return false; - } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType, baseIndex; - - double doubleBasePosition[3]; - double doubleBaseInertialFramePosition[3]; - scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition); - scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition); - - double doubleBaseOrientation[4]; - double doubleBaseInertialFrameOrientation[4]; - scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation); - scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation); - - command = b3CreateMultiBodyCommandInit(sm); - - baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex, - doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation); - - for (int i = 0; i < args.m_numLinks; i++) { - double linkMass = args.m_linkMasses[i]; - int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i]; - int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i]; - b3Vector3 linkPosition = args.m_linkPositions[i]; - b3Quaternion linkOrientation = args.m_linkOrientations[i]; - b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i]; - b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i]; - int linkParentIndex = args.m_linkParentIndices[i]; - int linkJointType = args.m_linkJointTypes[i]; - b3Vector3 linkJointAxis = args.m_linkJointAxes[i]; - - double doubleLinkPosition[3]; - double doubleLinkInertialFramePosition[3]; - double doubleLinkJointAxis[3]; - scalarToDouble3(linkPosition.m_floats, doubleLinkPosition); - scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition); - scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis); - - double doubleLinkOrientation[4]; - double doubleLinkInertialFrameOrientation[4]; - scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation); - scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation); - - b3CreateMultiBodyLink(command, - linkMass, - linkCollisionShapeIndex, - linkVisualShapeIndex, - doubleLinkPosition, - doubleLinkOrientation, - doubleLinkInertialFramePosition, - doubleLinkInertialFrameOrientation, - linkParentIndex, - linkJointType, - doubleLinkJointAxis - ); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) { - int uid = b3GetStatusBodyIndex(statusHandle); - return uid; - } - return -1; -} - -int b3RobotSimulatorClientAPI::getNumConstraints() const -{ - if (!isConnected()) { - b3Warning("Not connected"); - return -1; - } - return b3GetNumUserConstraints(m_data->m_physicsClientHandle); -} - -int b3RobotSimulatorClientAPI::getConstraintUniqueId(int serialIndex) -{ - if (!isConnected()) { - b3Warning("Not connected"); - return -1; - } - int userConstraintId = -1; - userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex); - return userConstraintId; -} \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 37e9c0e9a..a5f1a0cbd 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -1,578 +1,32 @@ -#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H -#define B3_ROBOT_SIMULATOR_CLIENT_API_H +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H -#include "../SharedMemory/SharedMemoryPublic.h" -#include "Bullet3Common/b3Vector3.h" -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3Transform.h" -#include "Bullet3Common/b3AlignedObjectArray.h" - -#include - -struct b3RobotSimulatorLoadUrdfFileArgs -{ - b3Vector3 m_startPosition; - b3Quaternion m_startOrientation; - bool m_forceOverrideFixedBase; - bool m_useMultiBody; - int m_flags; - - b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) - : m_startPosition(startPos), - m_startOrientation(startOrn), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) - { - } - - b3RobotSimulatorLoadUrdfFileArgs() - : m_startPosition(b3MakeVector3(0, 0, 0)), - m_startOrientation(b3Quaternion(0, 0, 0, 1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) - { - } -}; - -struct b3RobotSimulatorLoadSdfFileArgs -{ - bool m_forceOverrideFixedBase; - bool m_useMultiBody; - - b3RobotSimulatorLoadSdfFileArgs() - : m_forceOverrideFixedBase(false), - m_useMultiBody(true) - { - } -}; - -struct b3RobotSimulatorLoadFileResults -{ - b3AlignedObjectArray m_uniqueObjectIds; - b3RobotSimulatorLoadFileResults() - { - } -}; - -struct b3RobotSimulatorJointMotorArgs -{ - int m_controlMode; - - double m_targetPosition; - double m_kp; - - double m_targetVelocity; - double m_kd; - - double m_maxTorqueValue; - - b3RobotSimulatorJointMotorArgs(int controlMode) - : m_controlMode(controlMode), - m_targetPosition(0), - m_kp(0.1), - m_targetVelocity(0), - m_kd(0.9), - m_maxTorqueValue(1000) - { - } -}; - -enum b3RobotSimulatorInverseKinematicsFlags -{ - B3_HAS_IK_TARGET_ORIENTATION = 1, - B3_HAS_NULL_SPACE_VELOCITY = 2, - B3_HAS_JOINT_DAMPING = 4, -}; - -struct b3RobotSimulatorInverseKinematicArgs -{ - int m_bodyUniqueId; - // double* m_currentJointPositions; - // int m_numPositions; - double m_endEffectorTargetPosition[3]; - double m_endEffectorTargetOrientation[4]; - int m_endEffectorLinkIndex; - int m_flags; - int m_numDegreeOfFreedom; - b3AlignedObjectArray m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray m_jointDamping; - - b3RobotSimulatorInverseKinematicArgs() - : m_bodyUniqueId(-1), - m_endEffectorLinkIndex(-1), - m_flags(0) - { - m_endEffectorTargetPosition[0] = 0; - m_endEffectorTargetPosition[1] = 0; - m_endEffectorTargetPosition[2] = 0; - - m_endEffectorTargetOrientation[0] = 0; - m_endEffectorTargetOrientation[1] = 0; - m_endEffectorTargetOrientation[2] = 0; - m_endEffectorTargetOrientation[3] = 1; - } -}; - -struct b3RobotSimulatorInverseKinematicsResults -{ - int m_bodyUniqueId; - b3AlignedObjectArray m_calculatedJointPositions; -}; - -struct b3JointStates2 -{ - int m_bodyUniqueId; - int m_numDegreeOfFreedomQ; - int m_numDegreeOfFreedomU; - b3Transform m_rootLocalInertialFrame; - b3AlignedObjectArray m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray m_jointReactionForces; -}; +#include "b3RobotSimulatorClientAPI_NoGUI.h" -struct b3RobotSimulatorJointMotorArrayArgs -{ - int m_controlMode; - int m_numControlledDofs; - - int *m_jointIndices; - - double *m_targetPositions; - double *m_kps; - - double *m_targetVelocities; - double *m_kds; - - double *m_forces; - - b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) - : m_controlMode(controlMode), m_numControlledDofs(numControlledDofs) - { - } -}; - -struct b3RobotSimulatorGetCameraImageArgs -{ - int m_width; - int m_height; - float *m_viewMatrix; - float *m_projectionMatrix; - float *m_lightDirection; - float *m_lightColor; - float m_lightDistance; - int m_hasShadow; - float m_lightAmbientCoeff; - float m_lightDiffuseCoeff; - float m_lightSpecularCoeff; - int m_renderer; - - b3RobotSimulatorGetCameraImageArgs(int width, int height) - : m_width(width), - m_height(height), - m_viewMatrix(NULL), - m_projectionMatrix(NULL), - m_lightDirection(NULL), - m_lightColor(NULL), - m_lightDistance(-1), - m_hasShadow(-1), - m_lightAmbientCoeff(-1), - m_lightDiffuseCoeff(-1), - m_lightSpecularCoeff(-1), - m_renderer(-1) - { - } -}; - -struct b3RobotSimulatorSetPhysicsEngineParameters -{ - double m_fixedTimeStep; - int m_numSolverIterations; - int m_useSplitImpulse; - double m_splitImpulsePenetrationThreshold; - int m_numSubSteps; - int m_collisionFilterMode; - double m_contactBreakingThreshold; - int m_maxNumCmdPer1ms; - int m_enableFileCaching; - double m_restitutionVelocityThreshold; - double m_erp; - double m_contactERP; - double m_frictionERP; - - b3RobotSimulatorSetPhysicsEngineParameters() - : m_fixedTimeStep(-1), - m_numSolverIterations(-1), - m_useSplitImpulse(-1), - m_splitImpulsePenetrationThreshold(-1), - m_numSubSteps(-1), - m_collisionFilterMode(-1), - m_contactBreakingThreshold(-1), - m_maxNumCmdPer1ms(-1), - m_enableFileCaching(-1), - m_restitutionVelocityThreshold(-1), - m_erp(-1), - m_contactERP(-1), - m_frictionERP(-1) - {} -}; - -struct b3RobotSimulatorChangeDynamicsArgs -{ - double m_mass; - double m_lateralFriction; - double m_spinningFriction; - double m_rollingFriction; - double m_restitution; - double m_linearDamping; - double m_angularDamping; - double m_contactStiffness; - double m_contactDamping; - int m_frictionAnchor; - - b3RobotSimulatorChangeDynamicsArgs() - : m_mass(-1), - m_lateralFriction(-1), - m_spinningFriction(-1), - m_rollingFriction(-1), - m_restitution(-1), - m_linearDamping(-1), - m_angularDamping(-1), - m_contactStiffness(-1), - m_contactDamping(-1), - m_frictionAnchor(-1) - {} -}; - -struct b3RobotSimulatorAddUserDebugLineArgs -{ - double m_colorRGB[3]; - double m_lineWidth; - double m_lifeTime; - int m_parentObjectUniqueId; - int m_parentLinkIndex; - - b3RobotSimulatorAddUserDebugLineArgs() - : - m_lineWidth(1), - m_lifeTime(0), - m_parentObjectUniqueId(-1), - m_parentLinkIndex(-1) - { - m_colorRGB[0] = 1; - m_colorRGB[1] = 1; - m_colorRGB[2] = 1; - } -}; - -enum b3AddUserDebugTextFlags { - DEBUG_TEXT_HAS_ORIENTATION = 1 -}; - -struct b3RobotSimulatorAddUserDebugTextArgs -{ - double m_colorRGB[3]; - double m_size; - double m_lifeTime; - double m_textOrientation[4]; - int m_parentObjectUniqueId; - int m_parentLinkIndex; - int m_flags; - - b3RobotSimulatorAddUserDebugTextArgs() - : m_size(1), - m_lifeTime(0), - m_parentObjectUniqueId(-1), - m_parentLinkIndex(-1), - m_flags(0) - { - m_colorRGB[0] = 1; - m_colorRGB[1] = 1; - m_colorRGB[2] = 1; - - m_textOrientation[0] = 0; - m_textOrientation[1] = 0; - m_textOrientation[2] = 0; - m_textOrientation[3] = 1; - - } -}; - -struct b3RobotSimulatorGetContactPointsArgs -{ - int m_bodyUniqueIdA; - int m_bodyUniqueIdB; - int m_linkIndexA; - int m_linkIndexB; - - b3RobotSimulatorGetContactPointsArgs() - : m_bodyUniqueIdA(-1), - m_bodyUniqueIdB(-1), - m_linkIndexA(-2), - m_linkIndexB(-2) - {} -}; - -struct b3RobotSimulatorCreateCollisionShapeArgs -{ - int m_shapeType; - double m_radius; - b3Vector3 m_halfExtents; - double m_height; - char* m_fileName; - b3Vector3 m_meshScale; - b3Vector3 m_planeNormal; - int m_flags; - b3RobotSimulatorCreateCollisionShapeArgs() - : m_shapeType(-1), - m_radius(0.5), - m_height(1), - m_fileName(NULL), - m_flags(0) - { - m_halfExtents.m_floats[0] = 1; - m_halfExtents.m_floats[1] = 1; - m_halfExtents.m_floats[2] = 1; - - m_meshScale.m_floats[0] = 1; - m_meshScale.m_floats[1] = 1; - m_meshScale.m_floats[2] = 1; - - m_planeNormal.m_floats[0] = 0; - m_planeNormal.m_floats[1] = 0; - m_planeNormal.m_floats[2] = 1; - } - -}; - -struct b3RobotSimulatorCreateMultiBodyArgs -{ - double m_baseMass; - int m_baseCollisionShapeIndex; - int m_baseVisualShapeIndex; - b3Vector3 m_basePosition; - b3Quaternion m_baseOrientation; - b3Vector3 m_baseInertialFramePosition; - b3Quaternion m_baseInertialFrameOrientation; - - int m_numLinks; - double *m_linkMasses; - int *m_linkCollisionShapeIndices; - int *m_linkVisualShapeIndices; - b3Vector3 *m_linkPositions; - b3Quaternion *m_linkOrientations; - b3Vector3 *m_linkInertialFramePositions; - b3Quaternion *m_linkInertialFrameOrientations; - int *m_linkParentIndices; - int *m_linkJointTypes; - b3Vector3 *m_linkJointAxes; - - int m_useMaximalCoordinates; - - b3RobotSimulatorCreateMultiBodyArgs() - : m_numLinks(0), m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_useMaximalCoordinates(0), - m_linkMasses(NULL), - m_linkCollisionShapeIndices(NULL), - m_linkVisualShapeIndices(NULL), - m_linkPositions(NULL), - m_linkOrientations(NULL), - m_linkInertialFramePositions(NULL), - m_linkInertialFrameOrientations(NULL), - m_linkParentIndices(NULL), - m_linkJointTypes(NULL), - m_linkJointAxes(NULL) - { - m_basePosition.setValue(0,0,0); - m_baseOrientation.setValue(0,0,0,1); - m_baseInertialFramePosition.setValue(0,0,0); - m_baseInertialFrameOrientation.setValue(0,0,0,1); - } -}; - -///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet +///The b3RobotSimulatorClientAPI_GUI is pretty much the C++ version of pybullet ///as documented in the pybullet Quickstart Guide ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA -class b3RobotSimulatorClientAPI +class b3RobotSimulatorClientAPI : public b3RobotSimulatorClientAPI_NoGUI { - struct b3RobotSimulatorClientAPI_InternalData* m_data; public: b3RobotSimulatorClientAPI(); + virtual ~b3RobotSimulatorClientAPI(); - bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); - - void disconnect(); - - bool isConnected() const; - - void setTimeOut(double timeOutInSec); - - void syncBodies(); - - void resetSimulation(); - - b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); - b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); - - int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs()); - bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); - bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); - bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); - - bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); - - bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; - bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); - - bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; - bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; - - int getNumJoints(int bodyUniqueId) const; - - bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); - - int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); - - int changeConstraint(int constraintId, b3JointInfo* jointInfo); - - void removeConstraint(int constraintId); - - bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); - - bool getJointStates(int bodyUniqueId, b3JointStates2& state); - - bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); - - void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); - - bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs, - int *jointIndices, double *targetVelocities, double *targetPositions, - double *forces, double *kps, double *kds); - - void stepSimulation(); - - bool canSubmitCommand() const; - - void setRealTimeSimulation(bool enableRealTimeSimulation); - - void setInternalSimFlags(int flags); - - void setGravity(const b3Vector3& gravityAcceleration); - - void setTimeStep(double timeStepInSeconds); - void setNumSimulationSubSteps(int numSubSteps); - void setNumSolverIterations(int numIterations); - void setContactBreakingThreshold(double threshold); - - bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); - - bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); - - bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); - - void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); - void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos); - - int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); - void stopStateLogging(int stateLoggerUniqueId); - - void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); - void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); - - void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); - - // JFC: added these 24 methods - - void getMouseEvents(b3MouseEventsData* mouseEventsData); - - bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeInverseKinematics, b3LinkState* linkState); - - bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData); - - bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput); - - int getNumBodies() const; - - int getBodyUniqueId(int bodyId) const; - - bool removeBody(int bodyUniqueId); - - bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo); - - bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args); - - int addUserDebugParameter(char *paramName, double rangeMin, double rangeMax, double startValue); - - double readUserDebugParameter(int itemUniqueId); - - bool removeUserDebugItem(int itemUniqueId); - - int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); - - int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); - - int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); - - int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); - - bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); - - bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args); - - bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags); - - bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags); - - bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags); - - bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags); - - bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable); - - bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo); - - bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo); - - bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo); - - bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData); - - bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData); - - bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax); - - bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax); - - int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); - - int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args); - - int getNumConstraints() const; - - int getConstraintUniqueId(int serialIndex); - - //////////////// INTERNAL - - void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin); - - //setGuiHelper is only used when embedded in existing example browser - void setGuiHelper(struct GUIHelperInterface* guiHelper); - //renderScene is only used when embedded in existing example browser + virtual bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + virtual void renderScene(); - //debugDraw is only used when embedded in existing example browser + virtual void debugDraw(int debugDrawMode); + virtual bool mouseMoveCallback(float x,float y); + virtual bool mouseButtonCallback(int button, int state, float x, float y); - ////////////////INTERNAL }; #endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h new file mode 100644 index 000000000..cac680fda --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h @@ -0,0 +1,18 @@ +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H + +#include "../SharedMemory/PhysicsClientC_API.h" + +struct b3RobotSimulatorClientAPI_InternalData +{ + b3PhysicsClientHandle m_physicsClientHandle; + struct GUIHelperInterface* m_guiHelper; + + b3RobotSimulatorClientAPI_InternalData() + : m_physicsClientHandle(0), + m_guiHelper(0) + { + } +}; + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp new file mode 100644 index 000000000..8beeed506 --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -0,0 +1,1997 @@ +#include "b3RobotSimulatorClientAPI_NoGUI.h" + + +#include "../SharedMemory/PhysicsClientC_API.h" +#include "b3RobotSimulatorClientAPI_InternalData.h" + +#ifdef BT_ENABLE_ENET +#include "../SharedMemory/PhysicsClientUDP_C_API.h" +#endif //PHYSICS_UDP + +#ifdef BT_ENABLE_CLSOCKET +#include "../SharedMemory/PhysicsClientTCP_C_API.h" +#endif //PHYSICS_TCP + +#include "../SharedMemory/PhysicsDirectC_API.h" + +#include "../SharedMemory/SharedMemoryPublic.h" +#include "Bullet3Common/b3Logging.h" + +static void scalarToDouble3(b3Scalar a[3], double b[3]) +{ + for (int i = 0; i < 3; i++) + { + b[i] = a[i]; + } +} + +static void scalarToDouble4(b3Scalar a[4], double b[4]) +{ + for (int i = 0; i < 4; i++) + { + b[i] = a[i]; + } +} + + +b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI() +{ + m_data = new b3RobotSimulatorClientAPI_InternalData(); +} + +b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI() +{ + delete m_data; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey) +{ + if (m_data->m_physicsClientHandle) + { + b3Warning("Already connected, disconnect first."); + return false; + } + b3PhysicsClientHandle sm = 0; + + int udpPort = 1234; + int tcpPort = 6667; + int key = SHARED_MEMORY_KEY; + bool connected = false; + + switch (mode) + { + + case eCONNECT_DIRECT: + { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: + { + if (portOrKey >= 0) + { + key = portOrKey; + } + sm = b3ConnectSharedMemory(key); + break; + } + case eCONNECT_UDP: + { + if (portOrKey >= 0) + { + udpPort = portOrKey; + } +#ifdef BT_ENABLE_ENET + + sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort); +#else + b3Warning("UDP is not enabled in this build"); +#endif //BT_ENABLE_ENET + + break; + } + case eCONNECT_TCP: + { + if (portOrKey >= 0) + { + tcpPort = portOrKey; + } +#ifdef BT_ENABLE_CLSOCKET + + sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort); +#else + b3Warning("TCP is not enabled in this pybullet build"); +#endif //BT_ENABLE_CLSOCKET + break; + } + + default: + { + b3Warning("connectPhysicsServer unexpected argument"); + } + }; + + if (sm) + { + m_data->m_physicsClientHandle = sm; + if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + disconnect(); + return false; + } + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::isConnected() const +{ + return (m_data->m_physicsClientHandle != 0); +} + +void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec); + +} + + +void b3RobotSimulatorClientAPI_NoGUI::disconnect() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3DisconnectSharedMemory(m_data->m_physicsClientHandle); + m_data->m_physicsClientHandle = 0; +} + +void b3RobotSimulatorClientAPI_NoGUI::syncBodies() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); +} + +void b3RobotSimulatorClientAPI_NoGUI::resetSimulation() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); +} + +bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const +{ + if (!isConnected()) + { + return false; + } + return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); +} + +void b3RobotSimulatorClientAPI_NoGUI::stepSimulation() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); + statusType = b3GetStatusType(statusHandle); + //b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED); + } +} + +void b3RobotSimulatorClientAPI_NoGUI::setGravity(const b3Vector3& gravityAcceleration) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + // b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + +b3Quaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) +{ + b3Quaternion q; + q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); + return q; +} + +b3Vector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const b3Quaternion& quat) +{ + b3Scalar roll,pitch,yaw; + quat.getEulerZYX(yaw,pitch,roll); + b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); + return rpy2; +} + +int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) +{ + int robotUniqueId = -1; + + if (!isConnected()) + { + b3Warning("Not connected"); + return robotUniqueId; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + + //setting the initial position, orientation and other arguments are optional + + b3LoadUrdfCommandSetFlags(command,args.m_flags); + + b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], + args.m_startPosition[1], + args.m_startPosition[2]); + b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); + if (args.m_forceOverrideFixedBase) + { + b3LoadUrdfCommandSetUseFixedBase(command, true); + } + b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + + b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + robotUniqueId = b3GetStatusBodyIndex(statusHandle); + } + return robotUniqueId; +} + +bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_MJCF_LOADING_COMPLETED) + { + b3Warning("Couldn't load .mjcf file."); + return false; + } + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); + if (numBodies) + { + results.m_uniqueObjectIds.resize(numBodies); + int numBodies; + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); + } + + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_BULLET_LOADING_COMPLETED) + { + return false; + } + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); + if (numBodies) + { + results.m_uniqueObjectIds.resize(numBodies); + int numBodies; + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); + } + + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + bool statusOk = false; + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); + if (statusType == CMD_SDF_LOADING_COMPLETED) + { + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); + if (numBodies) + { + results.m_uniqueObjectIds.resize(numBodies); + int numBodies; + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); + } + statusOk = true; + } + + return statusOk; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); + return (result != 0); +} + +bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); + + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + return false; + } + + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, &actualStateQ, + 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + + basePosition[0] = actualStateQ[0]; + basePosition[1] = actualStateQ[1]; + basePosition[2] = actualStateQ[2]; + + baseOrientation[0] = actualStateQ[3]; + baseOrientation[1] = actualStateQ[4]; + baseOrientation[2] = actualStateQ[5]; + baseOrientation[3] = actualStateQ[6]; + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle commandHandle; + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); + b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], + baseOrientation[2], baseOrientation[3]); + + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + const int status_type = b3GetStatusType(statusHandle); + const double* actualStateQdot; + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + return false; + } + + b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3Vector3DoubleData linVelDouble; + linearVelocity.serializeDouble(linVelDouble); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); + + b3Vector3DoubleData angVelDouble; + angularVelocity.serializeDouble(angVelDouble); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + return true; +} + +void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetInternalSimFlags(command, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + } +} + + +void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeSimulation) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + + int ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + +int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); +} + +bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); +} + +int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + b3SharedMemoryStatusHandle statusHandle; + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_CONSTRAINT_COMPLETED) + { + int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); + return userConstraintUid; + } + } + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointInfo* jointInfo) +{ + + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); + + if (jointInfo->m_flags & eJointChangeMaxForce) + { + b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce); + } + + if (jointInfo->m_flags & eJointChangeChildFramePosition) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]); + } + if (jointInfo->m_flags & eJointChangeChildFrameOrientation) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]); + } + + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + int statusType = b3GetStatusType(statusHandle); + return statusType; +} + +void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + int statusType = b3GetStatusType(statusHandle); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) + { + return true; + } + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointStates2& state) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (statusHandle) + { + // double rootInertialFrame[7]; + const double* rootLocalInertialFrame; + const double* actualStateQ; + const double* actualStateQdot; + const double* jointReactionForces; + + int stat = b3GetStatusActualState(statusHandle, + &state.m_bodyUniqueId, + &state.m_numDegreeOfFreedomQ, + &state.m_numDegreeOfFreedomU, + &rootLocalInertialFrame, + &actualStateQ, + &actualStateQdot, + &jointReactionForces); + if (stat) + { + state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); + state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); + + for (int i=0;im_physicsClientHandle, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + return false; + } + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + return false; +} + +void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + switch (args.m_controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + b3JointControlSetKd(command, uIndex, args.m_kd); + b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); + b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + int qIndex = jointInfo.m_qIndex; + + b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition); + b3JointControlSetKp(command, uIndex, args.m_kp); + b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); + b3JointControlSetKd(command, uIndex, args.m_kd); + b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + break; + } + case CONTROL_MODE_TORQUE: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + break; + } + default: + { + b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl"); + } + } +} + +void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSolverIterations(command, numIterations); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + +void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double threshold) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetContactBreakingThreshold(command,threshold); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + + +void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + int ret; + ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); +} + +void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + +bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3Assert(args.m_endEffectorLinkIndex >= 0); + b3Assert(args.m_bodyUniqueId >= 0); + + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId); + if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) + { + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } + else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation); + } + else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } + else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition); + } + + if (args.m_flags & B3_HAS_JOINT_DAMPING) + { + b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); + } + + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + int numPos = 0; + + bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &numPos, + 0) != 0; + if (result && numPos) + { + results.m_calculatedJointPositions.resize(numPos); + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &numPos, + &results.m_calculatedJointPositions[0]) != 0; + } + return result; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) + { + int dofCount; + b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian); + return true; + } + return false; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) +{ + bool computeLinkVelocity = true; + bool computeForwardKinematics = true; + + return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState); +} + +bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + if (computeLinkVelocity) { + b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity); + } + + if (computeForwardKinematics) { + b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics); + } + + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + + if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); + return true; + } + return false; +} + +void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); + b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + +void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) +{ + vrEventsData->m_numControllerEvents = 0; + vrEventsData->m_controllerEvents = 0; + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); + b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); +} + +void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = 0; + keyboardEventsData->m_keyboardEvents = 0; + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); +} + +int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + int loggingUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); + + b3StateLoggingStart(commandHandle, loggingType, fileName.c_str()); + + for (int i = 0; i < objectUniqueIds.size(); i++) + { + int objectUid = objectUniqueIds[i]; + b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid); + } + + if (maxLogDof > 0) + { + b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_STATE_LOGGING_START_COMPLETED) + { + loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); + } + return loggingUniqueId; +} + +void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); + b3StateLoggingStop(commandHandle, stateLoggerUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + +void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); + if (commandHandle) + { + if ((cameraDistance >= 0)) + { + b3Vector3FloatData camTargetPos; + targetPos.serializeFloat(camTargetPos); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + } +} + +void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str()); + if (durationInMicroSeconds>=0) + { + b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + +void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSoftBodySetScale(command, scale); + b3LoadSoftBodySetMass(command, mass); + b3LoadSoftBodySetCollisionMargin(command, collisionMargin); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); +} + +void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = 0; + mouseEventsData->m_mouseEvents = 0; + if (!isConnected()) { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, + double *jointAccelerations, double *jointForcesOutput) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + + int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, + jointVelocities, jointAccelerations); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0); + + if (dofCount) { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput); + return true; + } + } + return false; +} + +int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + return b3GetNumBodies(m_data->m_physicsClientHandle); +} + +int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId); +} + +bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId)); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_REMOVE_BODY_COMPLETED) { + return true; + } else { + b3Warning("getDynamicsInfo did not complete"); + return false; + } + } + b3Warning("removeBody could not submit command"); + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) { + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + // struct b3DynamicsInfo info; + + if (bodyUniqueId < 0) { + b3Warning("getDynamicsInfo failed; invalid bodyUniqueId"); + return false; + } + if (linkIndex < -1) { + b3Warning("getDynamicsInfo failed; invalid linkIndex"); + return false; + } + + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { + cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex); + status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) { + return true; + } else { + b3Warning("getDynamicsInfo did not complete"); + return false; + } + } + b3Warning("getDynamicsInfo could not submit command"); + return false; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (args.m_mass >= 0) { + b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass); + } + + if (args.m_lateralFriction >= 0) { + b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction); + } + + if (args.m_spinningFriction>=0) { + b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction); + } + + if (args.m_rollingFriction>=0) { + b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction); + } + + if (args.m_linearDamping>=0) { + b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping); + } + + if (args.m_angularDamping>=0) { + b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping); + } + + if (args.m_restitution>=0) { + b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution); + } + + if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) { + b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping); + } + + if (args.m_frictionAnchor>=0) { + b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugParameter failed."); + return -1; +} + + +double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return 0; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) { + double paramValue = 0.f; + int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue); + if (ok) { + return paramValue; + } + } + b3Warning("readUserDebugParameter failed."); + return 0; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + return true; +} + + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime); + + if (args.m_parentObjectUniqueId>=0) { + b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); + } + + if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) { + b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugText3D failed."); + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +{ + double dposXYZ[3]; + dposXYZ[0] = posXYZ.x; + dposXYZ[1] = posXYZ.y; + dposXYZ[2] = posXYZ.z; + + return addUserDebugText(text, &dposXYZ[0], args); +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime); + + if (args.m_parentObjectUniqueId>=0) { + b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugLine failed."); + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +{ + double dfromXYZ[3]; + double dtoXYZ[3]; + dfromXYZ[0] = fromXYZ.x; + dfromXYZ[1] = fromXYZ.y; + dfromXYZ[2] = fromXYZ.z; + + dtoXYZ[0] = toXYZ.x; + dtoXYZ[1] = toXYZ.y; + dtoXYZ[2] = toXYZ.z; + return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args); +} + + + +bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + // int statusType; + struct b3JointInfo info; + + commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode); + + for (int i=0;im_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (args.m_numSolverIterations >= 0) { + b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations); + } + + if (args.m_collisionFilterMode >= 0) { + b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode); + } + + if (args.m_numSubSteps >= 0) { + b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps); + } + + if (args.m_fixedTimeStep >= 0) { + b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep); + } + + if (args.m_useSplitImpulse >= 0) { + b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse); + } + + if (args.m_splitImpulsePenetrationThreshold >= 0) { + b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold); + } + + if (args.m_contactBreakingThreshold >= 0) { + b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold); + } + + if (args.m_maxNumCmdPer1ms >= -1) { + b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms); + } + + if (args.m_restitutionVelocityThreshold>=0) { + b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold); + } + + if (args.m_enableFileCaching>=0) { + b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching); + } + + if (args.m_erp>=0) { + b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp); + } + + if (args.m_contactERP>=0) { + b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP); + } + + if (args.m_frictionERP >=0) { + b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags) +{ + double dforce[3]; + double dposition[3]; + + dforce[0] = force.x; + dforce[1] = force.y; + dforce[2] = force.z; + + dposition[0] = position.x; + dposition[1] = position.y; + dposition[2] = position.z; + + return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags) +{ + double dtorque[3]; + + dtorque[0] = torque.x; + dtorque[1] = torque.y; + dtorque[2] = torque.z; + + return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex < 0) || (jointIndex >= numJoints)) { + b3Warning("Error: invalid jointIndex."); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3CreateSensorCommandInit(sm, bodyUniqueId); + b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CLIENT_COMMAND_COMPLETED) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitRequestOpenGLVisualizerCameraCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo); + + if (statusType) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitRequestContactPointInformation(sm); + + if (args.m_bodyUniqueIdA>=0) { + b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA); + } + if (args.m_bodyUniqueIdB>=0) { + b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB); + } + if (args.m_linkIndexA>=-1) { + b3SetContactFilterLinkA(command, args.m_linkIndexA); + } + if (args.m_linkIndexB >=-1) { + b3SetContactFilterLinkB(command, args.m_linkIndexB); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + b3GetContactPointInformation(sm, contactInfo); + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitClosestDistanceQuery(sm); + + b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA); + b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB); + b3SetClosestDistanceThreshold(command, distance); + + if (args.m_linkIndexA>=-1) { + b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA); + } + if (args.m_linkIndexB >=-1) { + b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + b3GetContactPointInformation(sm, contactInfo); + return true; + } + return false; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + // int statusType; + + command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + b3GetAABBOverlapResults(sm, overlapData); + + return true; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData) +{ + double daabbMin[3]; + double daabbMax[3]; + + daabbMin[0] = aabbMin.x; + daabbMin[1] = aabbMin.y; + daabbMin[2] = aabbMin.z; + + daabbMax[0] = aabbMax.x; + daabbMax[1] = aabbMax.y; + daabbMax[2] = aabbMax.z; + + return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData); +} + + + +bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (bodyUniqueId < 0) { + b3Warning("Invalid bodyUniqueId"); + return false; + } + + if (linkIndex < -1) { + b3Warning("Invalid linkIndex"); + return false; + } + + if (aabbMin == NULL || aabbMax == NULL) { + b3Warning("Output AABB matrix is NULL"); + return false; + } + + command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) { + return false; + } + if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax) +{ + double daabbMin[3]; + double daabbMax[3]; + + bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]); + + aabbMin.x = (float)daabbMin[0]; + aabbMin.y = (float)daabbMin[1]; + aabbMin.z = (float)daabbMin[2]; + + aabbMax.x = (float)daabbMax[0]; + aabbMax.y = (float)daabbMax[1]; + aabbMax.z = (float)daabbMax[2]; + + return status; +} + + +int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int shapeIndex = -1; + + command = b3CreateCollisionShapeCommandInit(sm); + + if (shapeType==GEOM_SPHERE && args.m_radius>0) { + shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius); + } + if (shapeType==GEOM_BOX) { + double halfExtents[3]; + scalarToDouble3(args.m_halfExtents, halfExtents); + shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents); + } + if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) { + shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height); + } + if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) { + shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height); + } + if (shapeType==GEOM_MESH && args.m_fileName) { + double meshScale[3]; + scalarToDouble3(args.m_meshScale, meshScale); + shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale); + } + if (shapeType==GEOM_PLANE) { + double planeConstant=0; + double planeNormal[3]; + scalarToDouble3(args.m_planeNormal, planeNormal); + shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant); + } + if (shapeIndex>=0 && args.m_flags) { + b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) { + int uid = b3GetStatusCollisionShapeUniqueId(statusHandle); + return uid; + } + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType, baseIndex; + + double doubleBasePosition[3]; + double doubleBaseInertialFramePosition[3]; + scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition); + scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition); + + double doubleBaseOrientation[4]; + double doubleBaseInertialFrameOrientation[4]; + scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation); + scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation); + + command = b3CreateMultiBodyCommandInit(sm); + + baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex, + doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation); + + for (int i = 0; i < args.m_numLinks; i++) { + double linkMass = args.m_linkMasses[i]; + int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i]; + int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i]; + b3Vector3 linkPosition = args.m_linkPositions[i]; + b3Quaternion linkOrientation = args.m_linkOrientations[i]; + b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i]; + b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i]; + int linkParentIndex = args.m_linkParentIndices[i]; + int linkJointType = args.m_linkJointTypes[i]; + b3Vector3 linkJointAxis = args.m_linkJointAxes[i]; + + double doubleLinkPosition[3]; + double doubleLinkInertialFramePosition[3]; + double doubleLinkJointAxis[3]; + scalarToDouble3(linkPosition.m_floats, doubleLinkPosition); + scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition); + scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis); + + double doubleLinkOrientation[4]; + double doubleLinkInertialFrameOrientation[4]; + scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation); + scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation); + + b3CreateMultiBodyLink(command, + linkMass, + linkCollisionShapeIndex, + linkVisualShapeIndex, + doubleLinkPosition, + doubleLinkOrientation, + doubleLinkInertialFramePosition, + doubleLinkInertialFrameOrientation, + linkParentIndex, + linkJointType, + doubleLinkJointAxis + ); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) { + int uid = b3GetStatusBodyIndex(statusHandle); + return uid; + } + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return -1; + } + return b3GetNumUserConstraints(m_data->m_physicsClientHandle); +} + +int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return -1; + } + int userConstraintId = -1; + userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex); + return userConstraintId; +} + + + +void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* guiHelper) +{ + m_data->m_guiHelper = guiHelper; +} + +struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper() +{ + return m_data->m_guiHelper; +} \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h new file mode 100644 index 000000000..229d058ec --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h @@ -0,0 +1,572 @@ +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_H + +#include "../SharedMemory/SharedMemoryPublic.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include + +struct b3RobotSimulatorLoadUrdfFileArgs +{ + b3Vector3 m_startPosition; + b3Quaternion m_startOrientation; + bool m_forceOverrideFixedBase; + bool m_useMultiBody; + int m_flags; + + b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) + : m_startPosition(startPos), + m_startOrientation(startOrn), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) + { + } + + b3RobotSimulatorLoadUrdfFileArgs() + : m_startPosition(b3MakeVector3(0, 0, 0)), + m_startOrientation(b3Quaternion(0, 0, 0, 1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) + { + } +}; + +struct b3RobotSimulatorLoadSdfFileArgs +{ + bool m_forceOverrideFixedBase; + bool m_useMultiBody; + + b3RobotSimulatorLoadSdfFileArgs() + : m_forceOverrideFixedBase(false), + m_useMultiBody(true) + { + } +}; + +struct b3RobotSimulatorLoadFileResults +{ + b3AlignedObjectArray m_uniqueObjectIds; + b3RobotSimulatorLoadFileResults() + { + } +}; + +struct b3RobotSimulatorJointMotorArgs +{ + int m_controlMode; + + double m_targetPosition; + double m_kp; + + double m_targetVelocity; + double m_kd; + + double m_maxTorqueValue; + + b3RobotSimulatorJointMotorArgs(int controlMode) + : m_controlMode(controlMode), + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.9), + m_maxTorqueValue(1000) + { + } +}; + +enum b3RobotSimulatorInverseKinematicsFlags +{ + B3_HAS_IK_TARGET_ORIENTATION = 1, + B3_HAS_NULL_SPACE_VELOCITY = 2, + B3_HAS_JOINT_DAMPING = 4, +}; + +struct b3RobotSimulatorInverseKinematicArgs +{ + int m_bodyUniqueId; + // double* m_currentJointPositions; + // int m_numPositions; + double m_endEffectorTargetPosition[3]; + double m_endEffectorTargetOrientation[4]; + int m_endEffectorLinkIndex; + int m_flags; + int m_numDegreeOfFreedom; + b3AlignedObjectArray m_lowerLimits; + b3AlignedObjectArray m_upperLimits; + b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; + b3AlignedObjectArray m_jointDamping; + + b3RobotSimulatorInverseKinematicArgs() + : m_bodyUniqueId(-1), + m_endEffectorLinkIndex(-1), + m_flags(0) + { + m_endEffectorTargetPosition[0] = 0; + m_endEffectorTargetPosition[1] = 0; + m_endEffectorTargetPosition[2] = 0; + + m_endEffectorTargetOrientation[0] = 0; + m_endEffectorTargetOrientation[1] = 0; + m_endEffectorTargetOrientation[2] = 0; + m_endEffectorTargetOrientation[3] = 1; + } +}; + +struct b3RobotSimulatorInverseKinematicsResults +{ + int m_bodyUniqueId; + b3AlignedObjectArray m_calculatedJointPositions; +}; + +struct b3JointStates2 +{ + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray m_jointReactionForces; +}; + + +struct b3RobotSimulatorJointMotorArrayArgs +{ + int m_controlMode; + int m_numControlledDofs; + + int *m_jointIndices; + + double *m_targetPositions; + double *m_kps; + + double *m_targetVelocities; + double *m_kds; + + double *m_forces; + + b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) + : m_controlMode(controlMode), m_numControlledDofs(numControlledDofs) + { + } +}; + +struct b3RobotSimulatorGetCameraImageArgs +{ + int m_width; + int m_height; + float *m_viewMatrix; + float *m_projectionMatrix; + float *m_lightDirection; + float *m_lightColor; + float m_lightDistance; + int m_hasShadow; + float m_lightAmbientCoeff; + float m_lightDiffuseCoeff; + float m_lightSpecularCoeff; + int m_renderer; + + b3RobotSimulatorGetCameraImageArgs(int width, int height) + : m_width(width), + m_height(height), + m_viewMatrix(NULL), + m_projectionMatrix(NULL), + m_lightDirection(NULL), + m_lightColor(NULL), + m_lightDistance(-1), + m_hasShadow(-1), + m_lightAmbientCoeff(-1), + m_lightDiffuseCoeff(-1), + m_lightSpecularCoeff(-1), + m_renderer(-1) + { + } +}; + +struct b3RobotSimulatorSetPhysicsEngineParameters +{ + double m_fixedTimeStep; + int m_numSolverIterations; + int m_useSplitImpulse; + double m_splitImpulsePenetrationThreshold; + int m_numSubSteps; + int m_collisionFilterMode; + double m_contactBreakingThreshold; + int m_maxNumCmdPer1ms; + int m_enableFileCaching; + double m_restitutionVelocityThreshold; + double m_erp; + double m_contactERP; + double m_frictionERP; + + b3RobotSimulatorSetPhysicsEngineParameters() + : m_fixedTimeStep(-1), + m_numSolverIterations(-1), + m_useSplitImpulse(-1), + m_splitImpulsePenetrationThreshold(-1), + m_numSubSteps(-1), + m_collisionFilterMode(-1), + m_contactBreakingThreshold(-1), + m_maxNumCmdPer1ms(-1), + m_enableFileCaching(-1), + m_restitutionVelocityThreshold(-1), + m_erp(-1), + m_contactERP(-1), + m_frictionERP(-1) + {} +}; + +struct b3RobotSimulatorChangeDynamicsArgs +{ + double m_mass; + double m_lateralFriction; + double m_spinningFriction; + double m_rollingFriction; + double m_restitution; + double m_linearDamping; + double m_angularDamping; + double m_contactStiffness; + double m_contactDamping; + int m_frictionAnchor; + + b3RobotSimulatorChangeDynamicsArgs() + : m_mass(-1), + m_lateralFriction(-1), + m_spinningFriction(-1), + m_rollingFriction(-1), + m_restitution(-1), + m_linearDamping(-1), + m_angularDamping(-1), + m_contactStiffness(-1), + m_contactDamping(-1), + m_frictionAnchor(-1) + {} +}; + +struct b3RobotSimulatorAddUserDebugLineArgs +{ + double m_colorRGB[3]; + double m_lineWidth; + double m_lifeTime; + int m_parentObjectUniqueId; + int m_parentLinkIndex; + + b3RobotSimulatorAddUserDebugLineArgs() + : + m_lineWidth(1), + m_lifeTime(0), + m_parentObjectUniqueId(-1), + m_parentLinkIndex(-1) + { + m_colorRGB[0] = 1; + m_colorRGB[1] = 1; + m_colorRGB[2] = 1; + } +}; + +enum b3AddUserDebugTextFlags { + DEBUG_TEXT_HAS_ORIENTATION = 1 +}; + +struct b3RobotSimulatorAddUserDebugTextArgs +{ + double m_colorRGB[3]; + double m_size; + double m_lifeTime; + double m_textOrientation[4]; + int m_parentObjectUniqueId; + int m_parentLinkIndex; + int m_flags; + + b3RobotSimulatorAddUserDebugTextArgs() + : m_size(1), + m_lifeTime(0), + m_parentObjectUniqueId(-1), + m_parentLinkIndex(-1), + m_flags(0) + { + m_colorRGB[0] = 1; + m_colorRGB[1] = 1; + m_colorRGB[2] = 1; + + m_textOrientation[0] = 0; + m_textOrientation[1] = 0; + m_textOrientation[2] = 0; + m_textOrientation[3] = 1; + + } +}; + +struct b3RobotSimulatorGetContactPointsArgs +{ + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + + b3RobotSimulatorGetContactPointsArgs() + : m_bodyUniqueIdA(-1), + m_bodyUniqueIdB(-1), + m_linkIndexA(-2), + m_linkIndexB(-2) + {} +}; + +struct b3RobotSimulatorCreateCollisionShapeArgs +{ + int m_shapeType; + double m_radius; + b3Vector3 m_halfExtents; + double m_height; + char* m_fileName; + b3Vector3 m_meshScale; + b3Vector3 m_planeNormal; + int m_flags; + b3RobotSimulatorCreateCollisionShapeArgs() + : m_shapeType(-1), + m_radius(0.5), + m_height(1), + m_fileName(NULL), + m_flags(0) + { + m_halfExtents.m_floats[0] = 1; + m_halfExtents.m_floats[1] = 1; + m_halfExtents.m_floats[2] = 1; + + m_meshScale.m_floats[0] = 1; + m_meshScale.m_floats[1] = 1; + m_meshScale.m_floats[2] = 1; + + m_planeNormal.m_floats[0] = 0; + m_planeNormal.m_floats[1] = 0; + m_planeNormal.m_floats[2] = 1; + } + +}; + +struct b3RobotSimulatorCreateMultiBodyArgs +{ + double m_baseMass; + int m_baseCollisionShapeIndex; + int m_baseVisualShapeIndex; + b3Vector3 m_basePosition; + b3Quaternion m_baseOrientation; + b3Vector3 m_baseInertialFramePosition; + b3Quaternion m_baseInertialFrameOrientation; + + int m_numLinks; + double *m_linkMasses; + int *m_linkCollisionShapeIndices; + int *m_linkVisualShapeIndices; + b3Vector3 *m_linkPositions; + b3Quaternion *m_linkOrientations; + b3Vector3 *m_linkInertialFramePositions; + b3Quaternion *m_linkInertialFrameOrientations; + int *m_linkParentIndices; + int *m_linkJointTypes; + b3Vector3 *m_linkJointAxes; + + int m_useMaximalCoordinates; + + b3RobotSimulatorCreateMultiBodyArgs() + : m_numLinks(0), m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_useMaximalCoordinates(0), + m_linkMasses(NULL), + m_linkCollisionShapeIndices(NULL), + m_linkVisualShapeIndices(NULL), + m_linkPositions(NULL), + m_linkOrientations(NULL), + m_linkInertialFramePositions(NULL), + m_linkInertialFrameOrientations(NULL), + m_linkParentIndices(NULL), + m_linkJointTypes(NULL), + m_linkJointAxes(NULL) + { + m_basePosition.setValue(0,0,0); + m_baseOrientation.setValue(0,0,0,1); + m_baseInertialFramePosition.setValue(0,0,0); + m_baseInertialFrameOrientation.setValue(0,0,0,1); + } +}; + + + +///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet +///as documented in the pybullet Quickstart Guide +///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA +class b3RobotSimulatorClientAPI_NoGUI +{ +protected: + + struct b3RobotSimulatorClientAPI_InternalData* m_data; + +public: + + b3RobotSimulatorClientAPI_NoGUI(); + virtual ~b3RobotSimulatorClientAPI_NoGUI(); + + bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + + void disconnect(); + + bool isConnected() const; + + void setTimeOut(double timeOutInSec); + + void syncBodies(); + + void resetSimulation(); + + b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); + b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); + + int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); + bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); + bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); + + bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); + + bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; + bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); + + bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; + bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; + + int getNumJoints(int bodyUniqueId) const; + + bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); + + int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + + int changeConstraint(int constraintId, b3JointInfo* jointInfo); + + void removeConstraint(int constraintId); + + bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); + + bool getJointStates(int bodyUniqueId, b3JointStates2& state); + + bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); + + void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); + + bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs, + int *jointIndices, double *targetVelocities, double *targetPositions, + double *forces, double *kps, double *kds); + + void stepSimulation(); + + bool canSubmitCommand() const; + + void setRealTimeSimulation(bool enableRealTimeSimulation); + + void setInternalSimFlags(int flags); + + void setGravity(const b3Vector3& gravityAcceleration); + + void setTimeStep(double timeStepInSeconds); + void setNumSimulationSubSteps(int numSubSteps); + void setNumSolverIterations(int numIterations); + void setContactBreakingThreshold(double threshold); + + bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); + + bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); + + bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); + + void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); + void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos); + + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); + void stopStateLogging(int stateLoggerUniqueId); + + void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); + void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); + + void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); + + // JFC: added these 24 methods + + void getMouseEvents(b3MouseEventsData* mouseEventsData); + + bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeInverseKinematics, b3LinkState* linkState); + + bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData); + + bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput); + + int getNumBodies() const; + + int getBodyUniqueId(int bodyId) const; + + bool removeBody(int bodyUniqueId); + + bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo); + + bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args); + + int addUserDebugParameter(char *paramName, double rangeMin, double rangeMax, double startValue); + + double readUserDebugParameter(int itemUniqueId); + + bool removeUserDebugItem(int itemUniqueId); + + int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + + int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + + int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + + int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + + bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); + + bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args); + + bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags); + + bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags); + + bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags); + + bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags); + + bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable); + + bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo); + + bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo); + + bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo); + + bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData); + + bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData); + + bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax); + + bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax); + + int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); + + int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args); + + int getNumConstraints() const; + + int getConstraintUniqueId(int serialIndex); + + void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin); + + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); + virtual struct GUIHelperInterface* getGuiHelper(); + +}; + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index 020ef2c64..f24ba6b90 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -193,6 +193,8 @@ if not _OPTIONS["no-enet"] then "RobotSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", + "b3RobotSimulatorClientAPI_NoGUI.cpp", + "b3RobotSimulatorClientAPI_NoGUI.h", "MinitaurSetup.cpp", "MinitaurSetup.h", myfiles @@ -283,6 +285,8 @@ project ("App_VRGloveHandSimulator") "VRGloveSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", + "b3RobotSimulatorClientAPI_NoGUI.cpp", + "b3RobotSimulatorClientAPI_NoGUI.h", myfiles } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 34f5d9f31..6dea73129 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -32,7 +32,6 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle); #include "PhysicsClientTCP_C_API.h" #endif -#include "SharedMemoryInProcessPhysicsC_API.h" #ifdef __cplusplus extern "C" { diff --git a/examples/TwoJoint/TwoJointMain.cpp b/examples/TwoJoint/TwoJointMain.cpp index 6c25fe5be..64e325bf5 100644 --- a/examples/TwoJoint/TwoJointMain.cpp +++ b/examples/TwoJoint/TwoJointMain.cpp @@ -9,6 +9,7 @@ #include "SharedMemory/PhysicsClientC_API.h" #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Quaternion.h" +#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h" extern const int CONTROL_RATE; const int CONTROL_RATE = 500; From b15ac0c5f63a521beaac926fc4718e6f7cc28bc5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Feb 2018 19:12:02 -0800 Subject: [PATCH 4/7] add pthread/DL to App_RobotSimulator_NoGUI for Linux (not Win32/Apple) --- examples/RobotSimulator/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index cb06becbd..2117dd332 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -109,6 +109,11 @@ IF(WIN32) IF(BUILD_ENET OR BUILD_CLSOCKET) TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI ws2_32 ) ENDIF(BUILD_ENET OR BUILD_CLSOCKET) +ELSE() + IF(APPLE) + ELSE(APPLE) + LINK_LIBRARIES( pthread ${DL} ) + ENDIF(APPLE) ENDIF(WIN32) TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath Bullet3Common) From 9b9a0c599210a434285a359f11fdaeab25993f30 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Feb 2018 19:38:39 -0800 Subject: [PATCH 5/7] fix #2 for pthread/dl --- Extras/BulletRobotics/CMakeLists.txt | 4 ++++ examples/RobotSimulator/CMakeLists.txt | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 8abac18f5..192799fa8 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -120,6 +120,10 @@ ELSE(WIN32) IF(BUILD_CLSOCKET) ADD_DEFINITIONS(${OSDEF}) ENDIF(BUILD_CLSOCKET) + + IF(NOT APPLE) + TARGET_LINK_LIBRARIES( pthread ${DL} ) + ENDIF(APPLE) ENDIF(WIN32) IF(BUILD_ENET) diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index 2117dd332..dfebf4b2d 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -112,7 +112,7 @@ IF(WIN32) ELSE() IF(APPLE) ELSE(APPLE) - LINK_LIBRARIES( pthread ${DL} ) + TARGET_LINK_LIBRARIES( pthread ${DL} ) ENDIF(APPLE) ENDIF(WIN32) From d4b834b9f03259cb0ad0d5e495a6dbf4c69b09b2 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Feb 2018 20:22:53 -0800 Subject: [PATCH 6/7] fix #3 --- Extras/BulletRobotics/CMakeLists.txt | 2 +- examples/RobotSimulator/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 192799fa8..b2c4523b6 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -122,7 +122,7 @@ ELSE(WIN32) ENDIF(BUILD_CLSOCKET) IF(NOT APPLE) - TARGET_LINK_LIBRARIES( pthread ${DL} ) + LINK_LIBRARIES( pthread ${DL} ) ENDIF(APPLE) ENDIF(WIN32) diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index dfebf4b2d..0647edf72 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -112,7 +112,7 @@ IF(WIN32) ELSE() IF(APPLE) ELSE(APPLE) - TARGET_LINK_LIBRARIES( pthread ${DL} ) + TARGET_LINK_LIBRARIES( App_RobotSimulator_NoGUI pthread ${DL} ) ENDIF(APPLE) ENDIF(WIN32) From ad3c236bfdc32368111d800cec8f52b055203612 Mon Sep 17 00:00:00 2001 From: Sam Wenke Date: Sat, 3 Feb 2018 12:51:43 -0500 Subject: [PATCH 7/7] pybullet support for gym.Env, including v0.9.x --- .../pybullet_envs/bullet/cartpole_bullet.py | 9 ++- .../gym/pybullet_envs/bullet/kukaCamGymEnv.py | 46 +++++++------- .../gym/pybullet_envs/bullet/kukaGymEnv.py | 61 +++++++++---------- .../bullet/minitaur_duck_gym_env.py | 5 ++ .../pybullet_envs/bullet/minitaur_gym_env.py | 5 ++ .../gym/pybullet_envs/bullet/racecarGymEnv.py | 37 ++++++----- .../pybullet_envs/bullet/racecarZEDGymEnv.py | 35 ++++++----- .../bullet/simpleHumanoidGymEnv.py | 21 ++++--- .../pybullet/gym/pybullet_envs/env_bases.py | 18 +++++- 9 files changed, 140 insertions(+), 97 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py index e28a9e962..277e52ba2 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py @@ -64,10 +64,10 @@ class CartPoleBulletEnv(gym.Env): # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state - + dv = 0.1 deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] - + p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) done = x < -self.x_threshold \ @@ -99,3 +99,8 @@ class CartPoleBulletEnv(gym.Env): def _render(self, mode='human', close=False): return + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py index 4e7d7e70e..8ed171be0 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py @@ -56,8 +56,8 @@ class KukaCamGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -74,15 +74,15 @@ class KukaCamGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.5 +0.2*random.random() ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -98,7 +98,7 @@ class KukaCamGymEnv(gym.Env): return [seed] def getExtendedObservation(self): - + #camEyePos = [0.03,0.236,0.54] #distance = 1.06 #pitch=-56 @@ -118,13 +118,13 @@ class KukaCamGymEnv(gym.Env): viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0] #projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] - + img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) rgb=img_arr[2] np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.01 @@ -142,7 +142,7 @@ class KukaCamGymEnv(gym.Env): realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -158,11 +158,11 @@ class KukaCamGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() reward = self._reward() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -190,18 +190,18 @@ class KukaCamGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -212,7 +212,7 @@ class KukaCamGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -227,18 +227,18 @@ class KukaCamGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) - reward = -1000 + reward = -1000 numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -254,3 +254,7 @@ class KukaCamGymEnv(gym.Env): #print(reward) return reward + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index b9ef16017..6fa57e76a 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -44,7 +44,7 @@ class KukaGymEnv(gym.Env): self._maxSteps = maxSteps self.terminated = 0 self._cam_dist = 1.3 - self._cam_yaw = 180 + self._cam_yaw = 180 self._cam_pitch = -40 self._p = p @@ -61,8 +61,8 @@ class KukaGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([largeValObservation] * observationDim) + + observation_high = np.array([largeValObservation] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -80,15 +80,15 @@ class KukaGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.55 +0.12*random.random() ypos = 0 +0.2*random.random() ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -115,7 +115,7 @@ class KukaGymEnv(gym.Env): dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]] dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]] dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]] - + gripperEul = p.getEulerFromQuaternion(gripperOrn) #print("gripperEul") #print(gripperEul) @@ -126,17 +126,17 @@ class KukaGymEnv(gym.Env): #print(projectedBlockPos2D) #print("blockEulerInGripper") #print(blockEulerInGripper) - + #we return the relative x,y position and euler angle of block in gripper space blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]] - + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) - + self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.005 @@ -154,7 +154,7 @@ class KukaGymEnv(gym.Env): f = 0.3 realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -168,7 +168,7 @@ class KukaGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]]) actionCost = np.linalg.norm(npaction)*10. @@ -177,9 +177,9 @@ class KukaGymEnv(gym.Env): reward = self._reward()-actionCost #print("reward") #print(reward) - + #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode="rgb_array", close=False): @@ -208,18 +208,18 @@ class KukaGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>self._maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("terminating, closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -230,7 +230,7 @@ class KukaGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -245,19 +245,19 @@ class KukaGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) reward = -1000 - + numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -276,10 +276,7 @@ class KukaGymEnv(gym.Env): #print(reward) return reward - def reset(self): - """Resets the state of the environment and returns an initial observation. - - Returns: observation (object): the initial observation of the - space. - """ - return self._reset() \ No newline at end of file + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py index 44b41dd7d..b5c3375b4 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py @@ -385,3 +385,8 @@ class MinitaurBulletDuckEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py index 38ec631a5..158128067 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -376,3 +376,8 @@ class MinitaurBulletEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index 7537d54c1..bdde5d963 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -50,8 +50,8 @@ class RacecarGymEnv(gym.Env): #self.reset() observationDim = 2 #len(self.getExtendedObservation()) #print("observationDim") - #print(observationDim) - # observation_high = np.array([np.finfo(np.float32).max] * observationDim) + #print(observationDim) + # observation_high = np.array([np.finfo(np.float32).max] * observationDim) observation_high = np.ones(observationDim) * 1000 #np.inf if (isDiscrete): self.action_space = spaces.Discrete(9) @@ -59,7 +59,7 @@ class RacecarGymEnv(gym.Env): action_dim = 2 self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) - self.action_space = spaces.Box(-action_high, action_high) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -74,14 +74,14 @@ class RacecarGymEnv(gym.Env): # pos,orn = self._p.getBasePositionAndOrientation(i) # newpos = [pos[0],pos[1],pos[2]-0.1] # self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -101,18 +101,18 @@ class RacecarGymEnv(gym.Env): def getExtendedObservation(self): self._observation = [] #self._racecar.getObservation() carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) - + self._observation.extend([ballPosInCar[0],ballPosInCar[1]]) return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + if (self._isDiscrete): fwd = [-1,-1,-1,0,0,0,1,1,1] steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] @@ -128,14 +128,14 @@ class RacecarGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -159,13 +159,13 @@ class RacecarGymEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array - + def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -174,3 +174,8 @@ class RacecarGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index d9546de8c..277453e14 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -40,7 +40,7 @@ class RacecarZEDGymEnv(gym.Env): self._renders = renders self._width = 100 self._height = 10 - + self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient( @@ -53,8 +53,8 @@ class RacecarZEDGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (isDiscrete): self.action_space = spaces.Discrete(9) else: @@ -77,14 +77,14 @@ class RacecarZEDGymEnv(gym.Env): pos,orn = self._p.getBasePositionAndOrientation(i) newpos = [pos[0],pos[1],pos[2]+0.1] self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2red.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -104,13 +104,13 @@ class RacecarZEDGymEnv(gym.Env): def getExtendedObservation(self): carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) carmat = self._p.getMatrixFromQuaternion(carorn) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) dist0 = 0.3 dist1 = 1. eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3] - targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] + targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] up = [carmat[2],carmat[5],carmat[8]] viewMat = self._p.computeViewMatrix(eyePos,targetPos,up) #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) @@ -122,12 +122,12 @@ class RacecarZEDGymEnv(gym.Env): np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + if (self._isDiscrete): fwd = [-1,-1,-1,0,0,0,1,1,1] steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] @@ -143,14 +143,14 @@ class RacecarZEDGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -177,10 +177,10 @@ class RacecarZEDGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -189,3 +189,8 @@ class RacecarZEDGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py index c632e7f31..53b19b7a8 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py @@ -46,8 +46,8 @@ class SimpleHumanoidGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -57,14 +57,14 @@ class SimpleHumanoidGymEnv(gym.Env): #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf")) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + p.setGravity(0,0,-10) self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -82,7 +82,7 @@ class SimpleHumanoidGymEnv(gym.Env): def getExtendedObservation(self): self._observation = self._humanoid.getObservation() return self._observation - + def _step(self, action): self._humanoid.applyAction(action) for i in range(self._actionRepeat): @@ -96,7 +96,7 @@ class SimpleHumanoidGymEnv(gym.Env): reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -104,8 +104,13 @@ class SimpleHumanoidGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): reward=self._humanoid.distance print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 5d49c22c3..ceafb6404 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -43,7 +43,7 @@ class MJCFBaseBulletEnv(gym.Env): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False - + self.physicsClientId = 0 else: self.ownsPhysicsClient = True @@ -75,12 +75,12 @@ class MJCFBaseBulletEnv(gym.Env): self.isRender = True if mode != "rgb_array": return np.array([]) - + base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz - + view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -100,6 +100,7 @@ class MJCFBaseBulletEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array + def _close(self): if (self.ownsPhysicsClient): if (self.physicsClientId>=0): @@ -109,6 +110,17 @@ class MJCFBaseBulletEnv(gym.Env): def HUD(self, state, a, done): pass + # backwards compatibility for gym >= v0.9.x + # for extension of this class. + def step(self, *args, **kwargs): + return self._step(*args, **kwargs) + + close = _close + render = _render + reset = _reset + seed = _seed + + class Camera: def __init__(self): pass