def _step(self, action):
p.stepSimulation()
# 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
force = action
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def getMotorTorques(self):
motorTorques = []
for i in range(self.nMotors):
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
motorTorques.append(jointState[3])
motorTorques = np.multiply(motorTorques, self.motorDir)
return motorTorques
def _step(self, action):
force = self.force_mag if action==1 else -self.force_mag
p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
p.stepSimulation()
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
done = bool(done)
reward = 1.0
#print("state=",self.state)
return np.array(self.state), reward, done, {}
def _step(self, action):
p.stepSimulation()
# 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 \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def GetMotorTorques(self):
"""Get the amount of torques the motors are exerting.
Returns:
Motor torques of all eight motors.
"""
if self._accurate_motor_model_enabled or self._pd_control_enabled:
return self._observed_motor_torques
else:
motor_torques = [
p.getJointState(self.minitaur, motor_id)[3]
for motor_id in self._motor_id_list
]
motor_torques = np.multiply(motor_torques, self._motor_direction)
return motor_torques
def reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"),
[0, 0, 0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0, 0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4, ))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(
self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def get_obs(self):
x, x_dot, theta, theta_dot = p.getJointState(
self.cartpole, 0)[0:2] + p.getJointState(self.cartpole, 1)[0:2]
# Clip velocities
x_dot = np.clip(x_dot / 3, -7, 7)
theta_dot = np.clip(theta_dot / 3, -7, 7)
# Change theta range to [-pi, pi]
if theta > 0:
if theta % (2 * np.pi) <= np.pi:
theta = theta % (2 * np.pi)
else:
theta = -np.pi + theta % np.pi
else:
if theta % (-2 * np.pi) >= -np.pi:
theta = theta % (-2 * np.pi)
else:
theta = np.pi + theta % -np.pi
theta /= np.pi
self.state = np.array([x, x_dot, theta, theta_dot])
return self.state
def state(self):
"""
:return: Current robot state, as a dictionary, containing
joint positions, velocities, efforts, zero jacobian,
joint space inertia tensor, end-effector position,
end-effector orientation, end-effector velocity (linear and angular),
end-effector force, end-effector torque
:rtype: dict: {'position': np.ndarray,
'velocity': np.ndarray,
'effort': np.ndarray,
'jacobian': np.ndarray,
'inertia': np.ndarray,
'ee_point': np.ndarray,
'ee_ori': np.ndarray,
'ee_vel': np.ndarray,
'ee_omg': np.ndarray,
'tip_state'['force']: np.ndarray,
'tip_state'['torque']: np.ndarray,
}
"""
joint_angles = self.angles()
joint_velocities = self.joint_velocities()
joint_efforts = self.joint_efforts()
state = {}
state['position'] = joint_angles
state['velocity'] = joint_velocities
state['effort'] = joint_efforts
state['jacobian'] = self.jacobian(None)
state['inertia'] = self.inertia(None)
state['ee_point'], state['ee_ori'] = self.ee_pose()
state['ee_vel'], state['ee_omg'] = self.ee_velocity()
tip_state = {}
ft_joint_state = pb.getJointState(self._id,
max(self._ft_joints),
physicsClientId=self._uid)
ft = np.asarray(ft_joint_state[2])
tip_state['force'] = ft[:3]
tip_state['torque'] = ft[3:]
state['tip_state'] = tip_state
return state
def get_potential(self, env):
"""
Compute task-specific potential: furniture joint positions
:param env: environment instance
:param: task potential
"""
task_potential = 0.0
for (body_id, joint_id) in self.body_joint_pairs:
j_type = p.getJointInfo(body_id, joint_id)[2]
j_pos = p.getJointState(body_id, joint_id)[0]
scale = self.prismatic_joint_reward_scale \
if j_type == p.JOINT_PRISMATIC \
else self.revolute_joint_reward_scale
task_potential += scale * j_pos
return task_potential
def motor_joint_accelerations(self):
""" returns the motor joint positions for "each DoF" according to pybullet.
Note: fixed joints have 0 degrees of freedoms.
"""
joint_states = []
for joint_num in range(self.num_joints):
joint_states.append(
pybullet.getJointState(self._arm_id,
joint_num,
physicsClientId=self.physics_id))
# Only get joint states of free joints
joint_accelerations = [state[3] for state in joint_states]
return joint_accelerations
def setAngle(self, id, angle, reach_time=0):
id = str(id)
if not (id in self.joint_id):
return
present_angle = bullet.getJointState(self.humanoid_id,
self.joint_id[id])[0]
target_angle = math.radians(angle)
max_vel = self.joint_max_vel[id] if reach_time == 0 else min(
abs(target_angle - present_angle) /
reach_time, self.joint_max_vel[id])
bullet.setJointMotorControl2(self.humanoid_id,
jointIndex=self.joint_id[id],
controlMode=bullet.POSITION_CONTROL,
targetPosition=target_angle,
force=7.3,
maxVelocity=max_vel)
def getObservation(self):
observation = []
pos, orn = p.getBasePositionAndOrientation(self.bioId)
euler = p.getEulerFromQuaternion(orn)
#observation.extend(list(pos))
#observation.extend(list(euler)) #roll, pitch, yaw
jointStates = []
jointPoses = []
for i in range(len(self.freeJointList)):
jointStates.append(
p.getJointState(self.bioId, self.freeJointList[i]))
jointPoses = [x[0] for x in jointStates]
observation.extend(list(jointPoses))
return observation
def getAngleInterpolation(bodyID, finalAngles, k=1000):
#starting angles are the angles that you begin at
#final angles are the angles you want to end at
#target angles is the big array that has the interpolation of all the angles between the starting and final angles
num_joints = p.getNumJoints(bodyID)
startingAngles = []
for i in range(num_joints):
startingAngles.append(p.getJointState(bodyID, i)[0])
startingAngles = np.array(startingAngles)
finalAngles = np.array(finalAngles)
targetAngles = np.zeros((k, num_joints))
targetAngles[0] = startingAngles
for i in range(1, k + 1):
targetAngles[
i - 1] = startingAngles + (finalAngles - startingAngles) * (i / k)
return targetAngles
def get_joint_torque(self, joint_uid):
"""Get the torque force of the joint.
This is the motor torque applied during the last stepSimulation().
Args:
joint_uid: A tuple of the body Unique ID and the joint index.
Returns:
A 3-dimensional float32 numpy array.
"""
body_uid, joint_ind = joint_uid
_, _, _, torque = pybullet.getJointState(bodyUniqueId=body_uid,
jointIndex=joint_ind,
physicsClientId=self.uid)
return np.array(torque, dtype=np.float32)
def _compute_observation(self):
humanoid_pos, humanoid_orientation = p.getBasePositionAndOrientation(
self.humanoid_id)
humanoid_orientation = p.getEulerFromQuaternion(humanoid_orientation)
humanoid_linear_vel, humanoid_angular_vel = p.getBaseVelocity(
self.humanoid_id)
joint_states = []
for j in self.joint_ids:
joint_states.append(p.getJointState(self.humanoid_id, j)[0])
return np.concatenate([
humanoid_pos, humanoid_orientation, humanoid_linear_vel,
humanoid_angular_vel, joint_states
])
def step(self, ctrl):
p.setJointMotorControl2(self.cartpole,
0,
p.TORQUE_CONTROL,
force=ctrl * 20)
p.stepSimulation()
self.step_ctr += 1
# x, x_dot, theta, theta_dot
obs = self.get_obs()
x, x_dot, theta, theta_dot = obs
x_sphere = x - np.sin(p.getJointState(self.cartpole, 1)[0])
target_pen = np.clip(
np.abs(x_sphere - self.target) * 3.0 * (1 - abs(theta)), -2, 2)
vel_pen = (np.square(x_dot) * 0.1 +
np.square(theta_dot) * 0.5) * (1 - abs(theta))
r = 1 - target_pen - vel_pen - np.square(ctrl[0]) * 0.03
#p.removeAllUserDebugItems()
#p.addUserDebugText("sphere mass: {0:.3f}".format(self.mass), [0, 0, 2])
#p.addUserDebugText("sphere x: {0:.3f}".format(x_sphere), [0, 0, 2])
#p.addUserDebugText("cart pen: {0:.3f}".format(cart_pen), [0, 0, 2])
#p.addUserDebugText("x: {0:.3f}".format(x), [0, 0, 2])
#p.addUserDebugText("x_target: {0:.3f}".format(self.target), [0, 0, 2.2])
#p.addUserDebugText("cart_pen: {0:.3f}".format(cart_pen), [0, 0, 2.4])
done = self.step_ctr > self.max_steps
# Change target
if np.random.rand() < self.target_change_prob:
self.target = np.clip(
np.random.rand() * 2 * self.target_var - self.target_var, -2,
2)
p.resetBasePositionAndOrientation(self.target_vis,
[self.target, 0, -1],
[0, 0, 0, 1])
if self.latent_input:
obs = np.concatenate((obs, [self.get_latent_label()]))
if self.action_input:
obs = np.concatenate((obs, ctrl))
obs = np.concatenate((obs, [self.target]))
return obs, r, done, {}
def get_joint_angle(self, leg_down):
rid = self.robot_id
if leg_down == 'left':
ang_a = p.getJointState(rid, 0)[0]
ang_b = p.getJointState(rid, 1)[0]
ang_c = p.getJointState(rid, 2)[0]
else:
ang_a = p.getJointState(rid, 4)[0]
ang_b = p.getJointState(rid, 5)[0]
ang_c = p.getJointState(rid, 6)[0]
return [ang_a, ang_b, ang_c]
def feedback(self, bodyID, state):
d = self.getD(bodyID, state)
v = getComVel(bodyID)
#print(v)
#print(d)
pos_torso = p.getJointState(bodyID, 2)[0]
#print("pos_torso:{}".format(pos_torso))
pose0 = np.array([-0.0, 0.0, -0.05, 0.2, 0.62 - pos_torso, -1.10, 0.2])
pose1 = np.array([-0.0, 0.0, -0.1, 0.2, -0.1 - pos_torso, -0.05, 0.2])
pose2 = np.array([-0.0, 0.62 - pos_torso, -1.10, 0.2, 0.0, -0.05, 0.2])
pose3 = np.array([-0.0, -0.1 - pos_torso, -0.05, 0.2, 0.0, -0.1, 0.2])
pose = []
pose.append(pose0)
pose.append(pose1)
pose.append(pose2)
pose.append(pose3)
for i in range(4):
self.states[i].setTargetAngles(pose[i])
if (state == 1):
self.states[state].setTargetAngles(pose[state])
self.states[state].targetAngles[4] += d * 2.5
#self.states[state].targetAngles[5] += d * 0.01 * np.pi
#self.states[state].targetAngles[6] += d * 0.1 * np.pi
#self.states[state].targetAngles[2] += d * 0.01 * np.pi
if (state == 3):
self.states[state].setTargetAngles(pose[state])
#self.states[state].targetAngles[0] += d * 0.03 * np.pi
#self.states[state].targetAngles[4] += d * 0.01 * np.pi
#self.states[state].targetAngles[5] += d * 0.01 * np.pi
self.states[state].targetAngles[1] += d * 2.5
#self.states[state].targetAngles[3] += d * 0.1 * np.pi
if (state == 0):
self.states[state].setTargetAngles(pose[state])
#self.states[state].targetAngles[0] += d * 0.03 * np.pi
# self.states[state].targetAngles[4] += d * 0.01 * np.pi
# self.states[state].targetAngles[5] += d * 0.01 * np.pi
self.states[state].targetAngles[4] += v * 0.1
# self.states[state].targetAngles[2] += d * 0.01 * np.pi'''
if (state == 2):
self.states[state].setTargetAngles(pose[state])
#self.states[state].targetAngles[0] += d * 0.03 * np.pi
# self.states[state].targetAngles[4] += d * 0.01 * np.pi
# self.states[state].targetAngles[5] += d * 0.01 * np.pi
self.states[state].targetAngles[1] += v * 0.1
def captureJointAction(self, t):
x = []
y = []
y2 = []
plt.title("Matplotlib demo")
plt.xlabel("x axis caption")
plt.ylabel("y axis caption")
for i in range(int(t // self.step_time)):
x.append(i)
info = bullet.getJointState(self.humanoid_id, self.joint_id['4'])
y.append(info[0])
y2.append(info[1])
bullet.stepSimulation()
time.sleep(self.step_time)
plt.plot(x, y, "ob")
plt.plot(x, y2, "or")
plt.show()
def getJointRanges(self):
lowerLimits, upperLimits, jointRanges, restPoses = [], [], [], []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.icubId, i)
if jointInfo[3] > -1:
ll, ul = jointInfo[8:10]
jr = ul - ll
# For simplicity, assume resting state == initial state
rp = p.getJointState(self.icubId, i)[0]
lowerLimits.append(ll)
upperLimits.append(ul)
jointRanges.append(jr)
restPoses.append(rp)
return lowerLimits, upperLimits, jointRanges, restPoses
def step(self):
goals = copy.copy(self.joint_state_goals)
self.apply_forces(goals)
bullet.stepSimulation()
self.sim_time += self.time_step
state_msg = JointState()
state_msg.position = []
state_msg.velocity = []
for joint in self.motor_joints:
joint_angle, joint_velocity, _, _ = bullet.getJointState(
self.robotID, joint)
state_msg.position.append(joint_angle)
state_msg.velocity.append(joint_velocity)
state_msg.header.stamp.sec = math.floor(self.sim_time)
state_msg.header.stamp.nanosec = int(
(self.sim_time * 1000000000) % 1000000000)
self._joint_state_pub.publish(state_msg)
def update_joint_drive(self):
max_vel = 2000
timestep = 1 / 240
kd = 0.1 # 0.1 # 0.13
gain_d = timestep * kd / (1 / 240)
if 'gain_d' in self.state.config:
gain_d = self.state.config['gain_d']
for i in range(12):
true_target = self.state.joint_target[i] + self.state.joint_offset[
i]
urdf_joint_id = self.urdf_joint_indexes[i]
kp = self.state.all_kp[i]
if i % 3 == 2:
kp *= 0.8
gain_p = timestep * timestep * kp / (0.27) # 0.003789
if 'gain_p' in self.state.config:
gain_p = self.state.config['gain_p']
deadband = self.state.joint_deadband[i]
data = p.getJointState(self.robotId,
urdf_joint_id,
physicsClientId=self.pcId)
cur_pos = data[0]
delta_target = cur_pos - true_target
if delta_target > deadband:
delta_target = deadband
elif delta_target < -deadband:
delta_target = -deadband
fake_target = true_target + delta_target
#f = 10 # 8 # abs(delta_target/deadband)*8
f = 10 * abs(delta_target / deadband)
self.state.fake_target[i] = fake_target
p.setJointMotorControl2(self.robotId,
urdf_joint_id,
p.POSITION_CONTROL,
targetPosition=fake_target,
force=f,
maxVelocity=max_vel,
positionGain=gain_p,
velocityGain=gain_d,
physicsClientId=self.pcId)
if self.debug:
self.to_plot[i + 40].append(data[0])
self.to_plot[i + 40 + 12].append(data[3])
def get_reward(self, ee_state, pizza_state, action):
ee_pos = np.array(ee_state[4])
ee_orn = ee_state[5]
pizza_pos, pizza_orn = pizza_state[0]
pizza_pos = np.array(pizza_pos)
pizza_linear_vel, pizza_angular_vel = pizza_state[1]
pan_reward = -np.linalg.norm(ee_orn - pan_default_orn)**2
catch_rew = -0.1 * np.linalg.norm(ee_pos - pizza_pos)**2
catch_rew += np.clip(pizza_pos[2] - ee_pos[2], -np.inf, 0.)
flip_rew = -np.linalg.norm(target_flip_orn - pizza_orn)**2
if flip_rew > self._max_flip_reward:
self._max_flip_reward = flip_rew
jnt_reward = 0.
for ctrl_idx in jnt_ctrl_idx:
th, thdot, thFx, thJx = bullet_client.getJointState(
self.robot_id, ctrl_idx)
jnt_reward += -(th - rp[ctrl_idx])**2 - 0.001 * (thdot)**2
return catch_rew + flip_rew + jnt_reward
def movj_speed_control(self, targj, speed=7.0):
currj = [p.getJointState(self.ur5, i)[0] for i in self.joints]
currj = np.array(currj)
diffj = targj - currj
if all(np.abs(diffj) < 0.1):
return False
norm = np.linalg.norm(diffj)
v = diffj / norm if norm > 0 else 0
stepj = diffj * speed
gains = np.ones(len(self.joints))
p.setJointMotorControlArray(bodyIndex=self.ur5,
jointIndices=self.joints,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=stepj,
velocityGains=gains)
p.stepSimulation()
def observe_state(self):
force_feedback = p.getJointState(self.pw.robot, self.finger_joints[0])[
2] #should be symmetric across fingers, hence picking one
binary_threshold = 1.5 #arbitrary really
binary_force_feedback = (np.abs(force_feedback) >
binary_threshold).astype(np.int32)
if self.force_feedback_type == None:
return self.get_pos()
elif self.force_feedback_type == "cont":
return np.hstack([self.get_pos(), force_feedback]).flatten()
elif self.force_feedback_type == "binary":
return np.hstack([self.get_pos(), binary_force_feedback]).flatten()
elif self.force_feedback_type == "discrete":
discrete_force_feedback = np.array(force_feedback).astype(np.int32)
return np.hstack([self.get_pos(),
discrete_force_feedback]).flatten()
else:
return self.get_pos()
def getJointState(self):
position_list = []
velocity_list = []
reactionForce_list = []
torque_list = []
for i in self.joint_id_list:
position, velocity, reactionForce, torque = p.getJointState(
self.robotId, i)
position_list.append(position)
velocity_list.append(velocity)
reactionForce_list.append(reactionForce)
torque_list.append(torque)
position_list = np.array(position_list)
velocity_list = np.array(velocity_list)
torque_list = np.array(torque_list)
reactionForce_list = np.array(reactionForce_list)
return position_list, velocity_list, torque_list, reactionForce_list
def get_joint_ranges(self, includeFixed=False):
all_joints = pyplan.get_movable_joints(self.id)
upper_limits = pyplan.get_max_limits(self.id, all_joints)
lower_limits = pyplan.get_min_limits(self.id, all_joints)
jointRanges, restPoses = [], []
numJoints = p.getNumJoints(self.id)
for i in range(numJoints):
jointInfo = p.getJointInfo(self.id, i)
if includeFixed or jointInfo[3] > -1:
rp = p.getJointState(self.id, i)[0]
jointRanges.append(2)
restPoses.append(rp)
return lower_limits, upper_limits, jointRanges, restPoses
def setRobot(self, robot, physicsClientId=None):
self._robot = robot
if physicsClientId != None:
self._physicsClientId = physicsClientId
joint_id_list = []
joint_pos_list = []
self._joint_number = 0
for i in range(p.getNumJoints(robot)):
jointInfo = p.getJointInfo(robot, i)
if jointInfo[2] == 0:
joint_id_list.append(jointInfo[0])
joint_pos_list.append(p.getJointState(robot, jointInfo[0])[0])
self._joint_number += 1
print(self._joint_number)
self._joint_id = np.array(joint_id_list, dtype=np.int32)
self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)
def set_joint_velocity(self, joint_uid, velocity):
"""Set the position of the joint.
The joint position will be set to the current position.
Args:
joint_uid: A tuple of the body Unique ID and the joint index.
vel: A float32 value.
"""
body_uid, joint_ind = joint_uid
position, _, _, _ = pybullet.getJointState(bodyUniqueId=body_uid,
jointIndex=joint_ind,
physicsClientId=self.uid)
pybullet.resetJointState(bodyUniqueId=body_uid,
jointIndex=joint_ind,
targetValue=position,
targetVelocity=velocity,
physicsClientId=self.uid)
def auto_close_gripper(self,
step: int = 120,
check_contact: bool = False) -> bool:
# Get initial gripper open position
initial_position = p.getJointState(
self.robot_id, self.joints[self.mimicParentName].id)[0]
initial_position = math.sin(0.715 - initial_position) * 0.1143 + 0.010
for step_idx in range(1, step):
current_target_open_length = initial_position - step_idx / step * initial_position
self.move_gripper(current_target_open_length, 1)
if current_target_open_length < 1e-5:
return False
# time.sleep(1 / 120)
if check_contact and self.gripper_contact():
return True
return False
def step_jointspace(self, actions):
"""
Executes a step in taskspace providing endeffector deltas dx, dy, dz and endeffector rotation
--------
Parameters:
actions: list(dx, dy, dz, dtheta)
"""
joint_pos = [
p.getJointState(self.o.kukaobject.kukaId, i)[0]
for i in range(p.getNumJoints(self.o.kukaobject.kukaId))
]
link_state = p.getLinkState(self.o.kukaobject.kukaId,
self.o.kukaobject.kukaEndEffectorIndex)
curr_eepos = link_state[0]
curr_orn = link_state[1]
tgt_eepos = list_add(list(curr_eepos), actions[:3])
tgt_rot = joint_pos[7] + actions[-1]
curr_euler_angle = p.getEulerFromQuaternion(curr_orn)
tgt_orn = list_add(
curr_orn,
p.getQuaternionFromEuler(
list_add(curr_euler_angle,
[actions[-2], actions[-3], actions[-1]])))
for i in range(self._numJoints):
p.setJointMotorControl2(bodyIndex=self.o.kukaobject.kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=joint_pos[i] + actions[i],
targetVelocity=0,
force=200,
positionGain=0.01,
velocityGain=0.35)
self.o.kukaobject.gripper_stabler_keep_commands()
for kuka_sec in range(1):
p.stepSimulation()
#if self.render:
if True:
time.sleep(0.001)
def showBestStanding(self, num=-1, log=0):
self.Disconnect()
self.RunSRV(1)
time.sleep(1)
'''print("\nInShow, {:.3f}".format(self.elites[-1][0][0]))'''
#angles = np.array([90-38+30, 180-83, 180-110, 63, 90-38+30, -(180-83), -(180-110), -63])*np.pi/180#self.population[sort[0]][0]
#botStartOrientation = pb.getQuaternionFromEuler([0,-30*np.pi/180,0])
#angles = np.array([90-38, 180-83, 180-110, 63, 90-38, -(180-83), -(180-110), -63])*np.pi/180
#botStartOrientation = pb.getQuaternionFromEuler([0,0,0])
#sort = np.argsort(-self.fitnesses)
#angles = self.population[sort[0]][0]
#botStartOrientation = pb.getQuaternionFromEuler([0, self.population[sort[0]][1], 0])
angles = self.elites[num][0]
botStartOrientation = pb.getQuaternionFromEuler(
[0, self.elites[num][1], 0])
cid, botId = self.Init(botStartOrientation, pb_type=pb.SHARED_MEMORY)
if log == 1:
logID = pb.startStateLogging(
loggingType=pb.STATE_LOGGING_VIDEO_MP4,
fileName="bestStanding.mp4")
print([cid, botId])
self.setLegs(angles, sleep=0, botID=botId, cid=cid)
self.Torques = []
for dur in range(0, 1000):
self.Step(1, sleep=1, cid=cid, botID=botId)
self.AdjustCamera(botID=botId, cid=cid)
pb.setJointMotorControlArray(botId,
range(8),
controlMode=pb.POSITION_CONTROL,
targetPositions=angles,
targetVelocities=[0] * 8,
forces=[99999999999] * 8)
torques = []
for j in range(8):
tmp, tmp, tmp, t = pb.getJointState(botId, j)
torques.append(t)
self.Torques.append(torques)
if log == 1:
pb.stopStateLogging(logID)
def _get_joint_list_info_and_state_as_dict(self):
jointType = {
0: "JOINT_REVOLUTE",
1: "JOINT_PRISMATIC",
2: "JOINT_SPHERICAL",
3: "JOINT_PLANAR",
4: "JOINT_FIXED"
}
#initialize jointList which is a list of dictionaries for every joint
lstJointInfoAndState = []
#get total number of joints in robot id
n_joints = p.getNumJoints(self.arm)
print("Total number of joints is ", n_joints)
for i in range(0, n_joints):
#get current joint
currJointInfo = p.getJointInfo(self.arm, i)
currJointState = p.getJointState(self.arm, i)
#write current joint info to dictionary
currJointInfo = {
"jointIndex": currJointInfo[0],
"jointName": currJointInfo[1],
"jointType": jointType[currJointInfo[2]],
"qIndex": currJointInfo[3],
"uIndex": currJointInfo[4],
"jointLowerLimit": currJointInfo[8],
"jointUpperLimit": currJointInfo[9],
"jointAxis": currJointInfo[13],
"parentFramePos": currJointInfo[14],
"parentIndex": currJointInfo[15]
}
currJointState = {
"jointPosition": currJointState[0],
"jointVelocity": currJointState[1],
"jointReactionForces": currJointState[2],
"appliedJointMotorTorque": currJointState[3]
}
currJointInfoAndState = currJointInfo.copy()
currJointInfoAndState.update(currJointState)
lstJointInfoAndState.append(currJointInfoAndState)
return lstJointInfoAndState
def get_joint_reaction_force(self, joint_uid):
"""Get the reaction force of the joint.
These are the joint reaction forces, if a torque sensor is enabled for
this joint it is [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is
[0, 0, 0, 0, 0, 0].
Args:
joint_uid: A tuple of the body Unique ID and the joint index.
Returns:
A 3-dimensional float32 numpy array.
"""
body_uid, joint_ind = joint_uid
_, _, reaction_force, _ = pybullet.getJointState(
bodyUniqueId=body_uid,
jointIndex=joint_ind,
physicsClientId=self.uid)
return np.array(reaction_force, dtype=np.float32)
for i in range(numLines):
spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011]
text = "Frame:" + str(frameNr) + "\nObject UID:" + objectInfo
textUid = p.addUserDebugText(text,
textColorRGB=[0, 0, 0],
textSize=0.02,
textPosition=textPos,
textOrientation=textOrn,
parentObjectUniqueId=uiCube,
parentLinkIndex=-1,
replaceItemUniqueId=lines[i])
lines[i] = textUid
#keep the gripper centered/symmetric
b = p.getJointState(pr2_gripper, 2)[0]
p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
events = p.getVREvents()
for e in (events):
if e[CONTROLLER_ID] == uiControllerId:
p.resetBasePositionAndOrientation(uiCube, e[POSITION], e[ORIENTATION])
pos = e[POSITION]
orn = e[ORIENTATION]
lineFrom = pos
mat = p.getMatrixFromQuaternion(orn)
dir = [mat[0], mat[3], mat[6]]
to = [pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen, pos[2] + dir[2] * rayLen]
hit = p.rayTest(lineFrom, to)
oldRay = pointRay
color = [1, 1, 0]
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1./500.)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
js = p.getJointState(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,(js[0]-jointOffsets[j])/jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
import pybullet as p
import time
p.connect(p.GUI)
cartpole=p.loadURDF("cartpole.urdf")
p.setRealTimeSimulation(1)
p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5)
while (1):
p.setGravity(0,0,-10)
js = p.getJointState(cartpole,1)
print("position=",js[0],"velocity=",js[1])
time.sleep(0.01)
targetVelocity=0,force=500,
positionGain=1,velocityGain=0.1)
#Tests that the robot can match the simulation position
#TODO: Move commands to different node so this doesn't slow down so much
rate = rospy.Rate(100)
while True:
msg = uc_msg()
p.stepSimulation()
i = 0
l_conf = []
r_conf = []
for joint in range(len(left_joints)):
l_conf.append(p.getJointState(pr2, left_joints[joint])[0])
for joint in range(len(right_joints)):
r_conf.append(p.getJointState(pr2, right_joints[joint])[0])
msg.l_shoulder_pan_joint = l_conf[0]
msg.l_shoulder_lift_joint = l_conf[1]
msg.l_upper_arm_roll_joint = l_conf[2]
msg.l_elbow_flex_joint = l_conf[3]
msg.l_forearm_roll_joint = l_conf[4]
msg.l_wrist_flex_joint = l_conf[5]
msg.l_wrist_roll_joint = l_conf[6]
msg.r_shoulder_pan_joint = r_conf[0]
msg.r_shoulder_lift_joint = r_conf[1]
msg.r_upper_arm_roll_joint = r_conf[2]
print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf",[0,0,-11])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))
p.setGravity(0,0,-10)
position_control = True
angle = math.pi*0.25
p.resetJointState(door,1,angle)
angleread = p.getJointState(door,1)
print("angleread = ",angleread)
prevTime = time.time()
angle = math.pi*0.5
count=0
while (1):
count+=1
if (count==12):
p.stopStateLogging(logId)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
curTime = time.time()
diff = curTime - prevTime
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01)
txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1])
prevTextId = textId
textId = p.addUserDebugText(txt, [0, 0, -0.2])
p.removeUserDebugItem(prevTextId)
import pybullet as p
p.connect(p.DIRECT)
hinge = p.loadURDF("hinge.urdf")
print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg")
hingeJointIndex = 0
#by default, joint motors are enabled, maintaining zero velocity
p.setJointMotorControl2(hinge, hingeJointIndex, p.VELOCITY_CONTROL, 0, 0, 0)
p.setGravity(0, 0, -10)
p.stepSimulation()
print("joint state without sensor")
print(p.getJointState(0, 0))
p.enableJointForceTorqueSensor(hinge, hingeJointIndex)
p.stepSimulation()
print("joint state with force/torque sensor, gravity [0,0,-10]")
print(p.getJointState(0, 0))
p.setGravity(0, 0, 0)
p.stepSimulation()
print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0, 0))
p.disconnect()
def get_state(self): x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex) return x, vx
