Python pybullet.getNumJoints() Examples
The following are 30
code examples of pybullet.getNumJoints().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
pybullet
, or try the search function
.
.
Example #1
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 7 votes |
|
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)
Example #2
| Source File: world_creation.py From assistive-gym with MIT License | 6 votes |
|
def enforce_joint_limits(self, body):
# Enforce joint limits
joint_states = p.getJointStates(body, jointIndices=list(range(p.getNumJoints(body, physicsClientId=self.id))), physicsClientId=self.id)
joint_positions = np.array([x[0] for x in joint_states])
lower_limits = []
upper_limits = []
for j in range(p.getNumJoints(body, physicsClientId=self.id)):
joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
joint_name = joint_info[1]
joint_pos = joint_positions[j]
lower_limit = joint_info[8]
upper_limit = joint_info[9]
if lower_limit == 0 and upper_limit == -1:
lower_limit = -1e10
upper_limit = 1e10
lower_limits.append(lower_limit)
upper_limits.append(upper_limit)
# print(joint_name, joint_pos, lower_limit, upper_limit)
if joint_pos < lower_limit:
p.resetJointState(body, jointIndex=j, targetValue=lower_limit, targetVelocity=0, physicsClientId=self.id)
elif joint_pos > upper_limit:
p.resetJointState(body, jointIndex=j, targetValue=upper_limit, targetVelocity=0, physicsClientId=self.id)
lower_limits = np.array(lower_limits)
upper_limits = np.array(upper_limits)
return lower_limits, upper_limits
Example #3
| Source File: VelocityHelper.py From bullet-gym with MIT License | 5 votes |
|
def __init__(self,body_id):
self.body_id = body_id
self.num_links = p.getNumJoints(self.body_id)
self.pose = np.zeros((self.num_links+1,7))
self.velocity = np.zeros((self.num_links+1,6))
self.update()
Example #4
| Source File: world_creation.py From assistive-gym with MIT License | 5 votes |
|
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05):
if self.human_impairment != 'tremor':
self.human_tremors = np.zeros(len(controllable_joints))
elif len(controllable_joints) == 4:
self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints))
else:
self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints))
# Set starting joint positions
human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
set_position = None
for j_index, j_angle in joints_positions:
if j == j_index:
p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id)
set_position = j_angle
break
if use_static_joints and j not in controllable_joints:
# Make all non controllable joints on the person static by setting mass of each link (joint) to 0
p.changeDynamics(human, j, mass=0, physicsClientId=self.id)
# Set velocities to 0
p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id)
# By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id)
self.enforce_joint_limits(human)
if human_reactive_force is not None:
# NOTE: This runs a Position / Velocity PD controller for each joint motor on the human
human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id)
target_human_joint_positions = np.array([x[0] for x in human_joint_states])
forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions)
p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
Example #5
| Source File: world_creation.py From assistive-gym with MIT License | 5 votes |
|
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0):
if left:
gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
else:
gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id)
if self.task == 'scratch_itch':
tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task == 'bed_bathing':
tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']:
if self.task == 'drinking':
visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj')
elif self.task in ['scooping', 'feeding']:
visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj')
elif self.task == 'arm_manipulation':
visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj')
collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj')
tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id)
tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id)
tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id)
if left:
# Disable collisions between the tool and robot
for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
else:
# Disable collisions between the tool and robot
for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
return tool
Example #6
| Source File: world_creation.py From assistive-gym with MIT License | 5 votes |
|
def print_joint_info(self, body, show_fixed=True):
joint_names = []
for j in range(p.getNumJoints(body, physicsClientId=self.id)):
if show_fixed or p.getJointInfo(body, j, physicsClientId=self.id)[2] != p.JOINT_FIXED:
print(p.getJointInfo(body, j, physicsClientId=self.id))
joint_names.append((j, p.getJointInfo(body, j, physicsClientId=self.id)[1]))
print(joint_names)
Example #7
| Source File: kuka.py From robotics-rl-srl with MIT License | 5 votes |
|
def reset(self):
"""
Reset the environment
"""
objects = p.loadSDF(os.path.join(self.urdf_root_path, "kuka_iiwa/kuka_with_gripper2.sdf"))
self.kuka_uid = objects[0]
p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, -0.15],
[0.000000, 0.000000, 0.000000, 1.000000])
self.joint_positions = [0.006418, 0.113184, -0.011401, -1.289317, 0.005379, 1.737684, -0.006539, 0.000048,
-0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200]
self.num_joints = p.getNumJoints(self.kuka_uid)
for jointIndex in range(self.num_joints):
p.resetJointState(self.kuka_uid, jointIndex, self.joint_positions[jointIndex])
p.setJointMotorControl2(self.kuka_uid, jointIndex, p.POSITION_CONTROL,
targetPosition=self.joint_positions[jointIndex], force=self.max_force)
self.end_effector_pos = np.array([0.537, 0.0, 0.5])
self.end_effector_angle = 0
self.motor_names = []
self.motor_indices = []
for i in range(self.num_joints):
joint_info = p.getJointInfo(self.kuka_uid, i)
q_index = joint_info[3]
if q_index > -1:
self.motor_names.append(str(joint_info[1]))
self.motor_indices.append(i)
Example #8
| Source File: abstract.py From costar_plan with Apache License 2.0 | 5 votes |
|
def findGraspFrame(self):
'''
Helper function to look up the grasp frame associated with a robot.
'''
joints = pb.getNumJoints(self.handle)
grasp_idx = None
for i in xrange(joints):
idx, name, jtype, qidx, \
uidx, flags, damping, friction, \
lower, upper, max_force, max_vel, \
link_name = pb.getJointInfo(self.handle, i)
#info = pb.getJointInfo(self.handle, i)
#print(info)
#link_name = info[12]
assert(isinstance(link_name, str))
if link_name == self.grasp_link:
grasp_idx = i
break
return grasp_idx
Example #9
| Source File: minitaur.py From midlevel-reps with MIT License | 5 votes |
|
def _BuildJointNameToIdDict(self):
num_joints = p.getNumJoints(self.minitaur)
self.joint_name_to_id = {}
for i in range(num_joints):
joint_info = p.getJointInfo(self.minitaur, i)
self.joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
Example #10
| Source File: MJCFCommon.py From bullet-gym with MIT License | 5 votes |
|
def getScene(self, bodies):
parts = {}
joints = {}
for i in range(len(bodies)):
for j in range(p.getNumJoints(bodies[i])):
_,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i],j)
joints[joint_name] = Joint(bodies,i,j)
joints[joint_name].disable_motor()
parts[part_name] = BodyPart(bodies,i,j)
return parts, joints
Example #11
| Source File: env.py From assistive-gym with MIT License | 5 votes |
|
def get_motor_joint_states(self, robot):
num_joints = p.getNumJoints(robot, physicsClientId=self.id)
joint_states = p.getJointStates(robot, range(num_joints), physicsClientId=self.id)
joint_infos = [p.getJointInfo(robot, i, physicsClientId=self.id) for i in range(num_joints)]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[2] != p.JOINT_FIXED]
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
Example #12
| Source File: RewardFunction.py From bullet-gym with MIT License | 5 votes |
|
def __init__(self, body_id, rewardType, config):
self.rewardType = rewardType
self.body_id = body_id
self.config = config
self.linkQty = p.getNumJoints(self.body_id) # the reward function will be calculated across n links
if(self.rewardType == self.VelocityReward):
self.lastPosition = np.zeros((self.linkQty+1,3))
for link_id in range(-1, self.linkQty):
self.lastPosition[link_id+1], orient = state_fields_of_pose_of(self.body_id, link_id)
Example #13
| Source File: Motionv0Env.py From bullet-gym with MIT License | 5 votes |
|
def get_discrete_action_space_of(body_id):
num_joints = p.getNumJoints(body_id)
return spaces.Discrete(3^num_joints) # here is the state explosion!
Example #14
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
self._robot = robot
self._physicsClientId = physicsClientId
jointNameToId = {}
joint_id_list = []
joint_pos_list = []
self._joint_number = 0
for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
if jointInfo[2] == 0:
joint_id_list.append(jointInfo[0])
joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self._joint_number += 1
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)
self._jointNameToId = jointNameToId
self._kp = kp
self._kd = kd
self._torque = torque
self._max_velocity = max_velocity
self._timeStep = timeStep
# print(self._joint_id)
# print(self._joint_targetPos)
# print(self._jointNameToId)
Example #15
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setRobot(self, robot, physicsClientId=None):
self._robot = robot
if physicsClientId != None:
self._physicsClientId = physicsClientId
jointNameToId = {}
joint_id_list = []
joint_pos_list = []
self._joint_number = 0
for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
if jointInfo[2] == 0:
joint_id_list.append(jointInfo[0])
joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self._joint_number += 1
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)
Example #16
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 5 votes |
|
def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
self._robot = robot
self._physicsClientId = physicsClientId
jointNameToId = {}
joint_id_list = []
joint_pos_list = []
self._joint_number = 0
for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
if jointInfo[2] == 0:
joint_id_list.append(jointInfo[0])
joint_pos_list.append(p.getJointState(
self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self._joint_number += 1
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)
self._jointNameToId = jointNameToId
self._kp = kp
self._kd = kd
self._torque = torque
self._max_velocity = max_velocity
self._timeStep = timeStep
# print(self._joint_id)
# print(self._joint_targetPos)
# print(self._jointNameToId)
Example #17
| Source File: panda_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
def reset(self):
# Load robot model
flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | p.URDF_USE_INERTIA_FROM_FILE
self.robot_id = p.loadURDF(os.path.join(franka_panda.get_data_path(), "panda_model.urdf"),
basePosition=self._base_position, useFixedBase=True, flags=flags,
physicsClientId=self._physics_client_id)
assert self.robot_id is not None, "Failed to load the panda model"
# reset joints to home position
num_joints = p.getNumJoints(self.robot_id, physicsClientId=self._physics_client_id)
idx = 0
for i in range(num_joints):
joint_info = p.getJointInfo(self.robot_id, i, physicsClientId=self._physics_client_id)
joint_name = joint_info[1].decode("UTF-8")
joint_type = joint_info[2]
if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
assert joint_name in self.initial_positions.keys()
self._joint_name_to_ids[joint_name] = i
p.resetJointState(self.robot_id, i, self.initial_positions[joint_name], physicsClientId=self._physics_client_id)
p.setJointMotorControl2(self.robot_id, i, p.POSITION_CONTROL,
targetPosition=self.initial_positions[joint_name],
positionGain=0.2, velocityGain=1.0,
physicsClientId=self._physics_client_id)
idx += 1
self.ll, self.ul, self.jr, self.rs = self.get_joint_ranges()
if self._use_IK:
self._home_hand_pose = [0.2, 0.0, 0.8,
min(m.pi, max(-m.pi, m.pi)),
min(m.pi, max(-m.pi, 0)),
min(m.pi, max(-m.pi, 0))]
self.apply_action(self._home_hand_pose)
p.stepSimulation(physicsClientId=self._physics_client_id)
Example #18
| Source File: bullet_physics_engine.py From NTP-vat-release with MIT License | 5 votes |
|
def get_joint_uids(body):
joint_uids = range(p.getNumJoints(body))
return joint_uids
Example #19
| Source File: bullet_physics_engine.py From NTP-vat-release with MIT License | 5 votes |
|
def get_link_uids(body):
# Links and Joints have the corresponding UIDs
link_uids = range(p.getNumJoints(body))
return link_uids
Example #20
| Source File: util.py From assistive-gym with MIT License | 5 votes |
|
def ik(self, body, target_joint, target_pos, target_orient, ik_indices=range(29, 29+7), max_iterations=1000, half_range=False):
key = '%d_%d' % (body, target_joint)
if key not in self.ik_lower_limits:
self.ik_lower_limits[key] = []
self.ik_upper_limits[key] = []
self.ik_joint_ranges[key] = []
self.ik_rest_poses[key] = []
j_names = []
for j in range(p.getNumJoints(body, physicsClientId=self.id)):
if p.getJointInfo(body, j, physicsClientId=self.id)[2] != p.JOINT_FIXED:
joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
lower_limit = joint_info[8]
upper_limit = joint_info[9]
# print(len(self.ik_lower_limits[key]), joint_info[1], lower_limit, upper_limit)
if lower_limit == 0 and upper_limit == -1:
# lower_limit = -1e10
# upper_limit = 1e10
lower_limit = -2*np.pi
upper_limit = 2*np.pi
self.ik_lower_limits[key].append(lower_limit)
self.ik_upper_limits[key].append(upper_limit)
if not half_range:
self.ik_joint_ranges[key].append(upper_limit - lower_limit)
else:
self.ik_joint_ranges[key].append((upper_limit - lower_limit)/2.0)
# self.ik_rest_poses[key].append((upper_limit + lower_limit)/2.0)
j_names.append([len(j_names)] + list(joint_info[:2]))
self.ik_rest_poses[key] = self.np_random.uniform(self.ik_lower_limits[key], self.ik_upper_limits[key]).tolist()
if target_orient is not None:
ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, targetOrientation=target_orient, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
else:
ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
# print(j_names)
# print(ik_joint_poses)
# exit()
target_joint_positions = ik_joint_poses[ik_indices]
return target_joint_positions
Example #21
| Source File: jacobian.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def getJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
Example #22
| Source File: simulation.py From onshape-to-robot with MIT License | 5 votes |
|
def autoCollisions(self):
"""Returns the total amount of N in autocollisions (not with ground)
Returns:
float -- Newtons of collisions not with ground
"""
total = 0
for k in range(1, p.getNumJoints(self.robot)):
contacts = p.getContactPoints(bodyA=k)
for contact in contacts:
if contact[2] != self.floor:
total += contact[9]
return total
Example #23
| Source File: jacobian.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def getMotorJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))] joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1] joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
Example #24
| Source File: baxter_ik_controller.py From robosuite with MIT License | 5 votes |
|
def setup_inverse_kinematics(self, urdf_path):
"""
This function is responsible for doing any setup for inverse kinematics.
Inverse Kinematics maps end effector (EEF) poses to joint angles that
are necessary to achieve those poses.
"""
# These indices come from the urdf file we're using
self.effector_right = 27
self.effector_left = 45
# Use PyBullet to handle inverse kinematics.
# Set up a connection to the PyBullet simulator.
p.connect(p.DIRECT)
p.resetSimulation()
self.ik_robot = p.loadURDF(urdf_path, (0, 0, 0), useFixedBase=1)
# Relevant joints we care about. Many of the joints are fixed and don't count, so
# we need this second map to use the right ones.
self.actual = [13, 14, 15, 16, 17, 19, 20, 31, 32, 33, 34, 35, 37, 38]
self.num_joints = p.getNumJoints(self.ik_robot)
n = p.getNumJoints(self.ik_robot)
self.rest = []
self.lower = []
self.upper = []
self.ranges = []
for i in range(n):
info = p.getJointInfo(self.ik_robot, i)
# Retrieve lower and upper ranges for each relevant joint
if info[3] > -1:
self.rest.append(p.getJointState(self.ik_robot, i)[0])
self.lower.append(info[8])
self.upper.append(info[9])
self.ranges.append(info[9] - info[8])
# Simulation will update as fast as it can in real time, instead of waiting for
# step commands like in the non-realtime case.
p.setRealTimeSimulation(1)
Example #25
| Source File: jacobian.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
Example #26
| Source File: agent.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def __init__(self):
# initialize robot
self.body = p.loadURDF("../soccer_description/models/soccerbot/model.xacro")
self.state = RobotState()
self.imu = -1
self.imuMeasurements = JointMeasurement()
self.joints = []
self.motors = []
# create a list of joints and find the IMU
for i in range(p.getNumJoints(self.body)):
self.joints.append(i)
jointInfo = p.getJointInfo(self.body, i)
if jointInfo[1].decode('ascii') == "torso_imu":
self.imu = jointInfo[0]
if self.imu == -1:
raise AttributeError("Could not find robot's imu sensor from joint list")
# rearrange joint order to match the order of positions found in the csv files. See:
# https://docs.google.com/spreadsheets/d/1KgIYwm3fNen8yjLEa-FEWq-GnRUnBjyg4z64nZ2uBv8/edit#gid=775054312
self.motors = self.joints[2:14] + self.joints[17:21] + self.joints[14:16] + self.joints[0:2]
# initialize animations
self.getupBackAnimation = Animation("./trajectories/getupback.csv")
self.getupFrontAnimation = Animation("./trajectories/getupfront.csv")
self.readyAnimation = Animation("./trajectories/ready.csv")
self.standingAnimation = Animation("./trajectories/standing.csv")
self.activeAnimation = None
Example #27
| Source File: saveRestoreStateTest.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf",useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10], useMaximalCoordinates=maximalCoordinates)
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
Example #28
| Source File: kuka.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
Example #29
| Source File: unittests.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def setJointPosition(self, robot, position, kp=1.0, kv=0.3):
import pybullet as p
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
Example #30
| Source File: unittests.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def getMotorJointStates(self, robot):
import pybullet as p
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
