def execute(self, joint_targets, gripper_targets):
# Iterate through target joint positions
for target in np.hstack((joint_targets, gripper_targets)):
# Set new target position
self.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(self.clientID)
# print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(
self.clientID)
# Calculate commands
commands = self.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(self.clientID, commands)
# Print current joint positions (comment out if you'd like)
# print(sensed_joint_positions)
vu.step_sim(self.clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = self.has_stably_converged_to_target()
def main(args):
success = False
try:
# Connect to V-REP
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
# Joint targets, radians for revolute joints and meters for prismatic joints
gripper_targets = np.asarray([[-0.03, 0.03], [-0.03, 0.03]])
joint_targets = np.radians([[-80, 0, 0, 0, 0], [0, 60, -75, -75, 0]])
# Verify fk
from utils import URDFRobot, print_pose
robot = URDFRobot('urdf/locobot_description_v3.urdf')
from forward_kinematics import ForwardKinematicsSolver
fksolver = ForwardKinematicsSolver(robot)
for target in joint_targets:
print(target)
pose = fksolver.getWristPose(target[:5], robot,
vu.ARM_JOINT_NAMES[:5])
print_pose(pose)
# Instantiate controller and run it
controller = ArmController(clientID)
controller.execute(joint_targets, gripper_targets)
success = True
except Exception as e:
print(e)
traceback.print_exc()
finally:
# Stop VREP simulation
vu.stop_sim(clientID)
# Plot time histories
if success: controller.plot()
def main(args):
global link_cuboid_spec
global obstacle_cuboid_spec
# Connect to V-REP
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
deg_to_rad = np.pi / 180.
link_cuboid_list = ("arm_base_link_joint_collision_cuboid",
"shoulder_link_collision_cuboid",
"elbow_link_collision_cuboid",
"forearm_link_collision_cuboid",
"wrist_link_collision_cuboid",
"gripper_link_collision_cuboid",
"finger_r_collision_cuboid",
"finger_l_collision_cuboid")
for link_cuboid in link_cuboid_list:
d = vu.get_handle_by_name(clientID, link_cuboid)
link_spec = {}
link_spec["Origin"] = vu.get_object_position(clientID, d)
link_spec["Orientation"] = vu.get_object_orientation(clientID, d)
link_spec["Dimension"] = vu.get_object_bounding_box(clientID, d)
link_cuboid_spec.append(link_spec)
obstacle_cuboid_list = ("cuboid_0", "cuboid_1", "cuboid_2", "cuboid_3",
"cuboid_4", "cuboid_5")
for obstacle_cuboid in obstacle_cuboid_list:
d = vu.get_handle_by_name(clientID, obstacle_cuboid)
obstacle_spec = {}
obstacle_spec["Origin"] = vu.get_object_position(clientID, d)
obstacle_spec["Orientation"] = vu.get_object_orientation(clientID, d)
obstacle_spec["Dimension"] = vu.get_object_bounding_box(clientID, d)
obstacle_cuboid_spec.append(obstacle_spec)
print("done")
def control(self, clientID, target):
self.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(clientID)
print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = self.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
print(sensed_joint_positions)
vu.step_sim(clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = self.has_stably_converged_to_target()
def main(args):
# Connect to V-REP
print ('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print ('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50.*np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
deg_to_rad = np.pi/180.
# Joint targets. Specify in radians for revolute joints and meters for prismatic joints.
# The order of the targets are as follows:
# joint_1 / revolute / arm_base_link <- shoulder_link
# joint_2 / revolute / shoulder_link <- elbow_link
# joint_3 / revolute / elbow_link <- forearm_link
# joint_4 / revolute / forearm_link <- wrist_link
# joint_5 / revolute / wrist_link <- gripper_link
# joint_6 / prismatic / gripper_link <- finger_r
# joint_7 / prismatic / gripper_link <- finger_l
joint_targets = [[ 0.,
0.,
0.,
0.,
0.,
- 0.07,
0.07], \
[-45.*deg_to_rad,
-15.*deg_to_rad,
20.*deg_to_rad,
15.*deg_to_rad,
-75.*deg_to_rad,
- 0.03,
0.03], \
[ 30.*deg_to_rad,
60.*deg_to_rad,
-65.*deg_to_rad,
45.*deg_to_rad,
0.*deg_to_rad,
- 0.05,
0.05]]
# Instantiate controller
controller = ArmController()
i=0
# Iterate through target joint positions
for target in joint_targets:
# Set new target position
controller.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
i+=1
timestamp = vu.get_sim_time_seconds(clientID)
print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = controller.calculate_commands_from_feedback(timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
#print(sensed_joint_positions)
vu.step_sim(clientID, 1)
if(i==1):
joint_pos = np.array(sensed_joint_positions)
target_pos = np.array(target)
time_history = np.array(timestamp)
else:
joint_pos = np.vstack((joint_pos,sensed_joint_positions))
target_pos = np.vstack((target_pos,target))
time_history = np.hstack((time_history,timestamp))
# Determine if we've met the condition to move on to the next point
steady_state_reached = controller.has_stably_converged_to_target()
vu.stop_sim(clientID)
plt.figure(1)
plt.subplot(331)
plt.xlabel('Timestamp (s)')
plt.ylabel('Angle(rad)')
plt.title('Joint 1 position over time')
plt.plot(time_history,joint_pos[:,0],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,0],'-r',label='Target position')
plt.subplot(332)
plt.xlabel('Timestamp (s)')
plt.ylabel('Angle(rad)')
plt.title('Joint 2 position over time')
plt.plot(time_history,joint_pos[:,1],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,1],'-r',label='Target position')
plt.subplot(333)
plt.xlabel('Timestamp (s)')
plt.ylabel('Angle(rad)')
plt.title('Joint 3 position over time')
plt.plot(time_history,joint_pos[:,2],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,2],'-r',label='Target position')
plt.subplot(334)
plt.xlabel('Timestamp (s)')
plt.ylabel('Angle(rad)')
plt.title('Joint 4 position over time')
plt.plot(time_history,joint_pos[:,3],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,3],'-r',label='Target position')
plt.subplot(335)
plt.xlabel('Timestamp (s)')
plt.ylabel('Angle(rad)')
plt.title('Joint 5 position over time')
plt.plot(time_history,joint_pos[:,4],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,4],'-r',label='Target position')
plt.subplot(336)
plt.xlabel('Timestamp (s)')
plt.ylabel('Position(m)')
plt.title('Joint 6 position over time')
plt.plot(time_history,joint_pos[:,5],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,5],'-r',label='Target position')
plt.subplot(337)
plt.xlabel('Timestamp (s)')
plt.ylabel('Position(m)')
plt.title('Joint 7 position over time')
plt.plot(time_history,joint_pos[:,6],'-b',label='Sensed position')
plt.plot(time_history,target_pos[:,6],'-r',label='Target position')
plt.show()
def main(args):
# Connect to V-REP
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
# Joint targets. Specify in radians for revolute joints and meters for prismatic joints.
# The order of the targets are as follows:
# joint_1 / revolute / arm_base_link <- shoulder_link
# joint_2 / revolute / shoulder_link <- elbow_link
# joint_3 / revolute / elbow_link <- forearm_link
# joint_4 / revolute / forearm_link <- wrist_link
# joint_5 / revolute / wrist_link <- gripper_link
# joint_6 / prismatic / gripper_link <- finger_r
# joint_7 / prismatic / gripper_link <- finger_l
joint_targets = [[0., 0., 0., 0., 0., -0.00, 0.00]]
# Instantiate controller
controller = ArmController()
# Iterate through target joint positions
for target in joint_targets:
# Set new target position
controller.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(clientID)
print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = controller.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
print(sensed_joint_positions)
vu.step_sim(clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = controller.has_stably_converged_to_target()
vu.stop_sim(clientID)
titles = [
'Joint_1', 'Joint_2', 'Joint_3', 'Joint_4', 'Joint_5', 'Joint_6',
'Joint_7'
]
for i in range(7):
plt.figure(i)
#plt.subplot(4,2,i+1)
plt.plot(np.asarray(controller.history['timestamp']),
np.asarray(controller.history['joint_feedback'])[:, i])
plt.plot(np.asarray(controller.history['timestamp']),
np.asarray(controller.history['joint_target'])[:, i], 'r--')
if (i < 5):
plt.ylabel('Joint Angle')
else:
plt.ylabel('Joint Position')
plt.xlabel('Time')
plt.title(titles[i])
plt.show()
def main(args):
# Connect to V-REP
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
deg_to_rad = np.pi / 180.
# Joint targets. Specify in radians for revolute joints and meters for prismatic joints.
# The order of the targets are as follows:
# joint_1 / revolute / arm_base_link <- shoulder_link
# joint_2 / revolute / shoulder_link <- elbow_link
# joint_3 / revolute / elbow_link <- forearm_link
# joint_4 / revolute / forearm_link <- wrist_link
# joint_5 / revolute / wrist_link <- gripper_link
# joint_6 / prismatic / gripper_link <- finger_r
# joint_7 / prismatic / gripper_link <- finger_l
joint_targets = [[ 0.,
0.,
0.,
0.,
0.,
- 0.07,
0.07], \
[-45.*deg_to_rad,
-15.*deg_to_rad,
20.*deg_to_rad,
15.*deg_to_rad,
-75.*deg_to_rad,
- 0.03,
0.03], \
[ 30.*deg_to_rad,
60.*deg_to_rad,
-65.*deg_to_rad,
45.*deg_to_rad,
0.*deg_to_rad,
- 0.05,
0.05]]
# Instantiate controller
controller = ArmController()
# Iterate through target joint positions
for target in joint_targets:
# Set new target position
controller.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(clientID)
print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = controller.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
print(sensed_joint_positions)
vu.step_sim(clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = controller.has_stably_converged_to_target()
vu.stop_sim(clientID)
vu.reset_sim(clientID)
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
deg_to_rad = np.pi / 180.
controller = locobot_joint_ctrl.ArmController()
obstacles = np.load('obstacles.npy')
prm = PRM.PRM_Map(obstacles)
start = np.array([[-80, 0, 0, 0, 0]])
target = np.array([[0, 60, -75, -75, 0]])
path = prm.Planner(start, target)
vu.step_sim(clientID)
for joints in path:
joints = (joints * deg_to_rad).tolist()
joints.append(-0.03)
joints.append(0.03)
print(joints)
controller.control(clientID, joints)
# Stop the simulation
vu.stop_sim(clientID)
# Use the below code to get the new obstacle, not recommended; Ensure to move all the joints to neutral position before saving
# obstacles = getCollisionCuboid(clientID,'obstacle')
# np.save('obstacles.npy', obstacles)
# Save_Joints(clientID)
def main(args):
# Connect to V-REP
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
deg_to_rad = np.pi / 180.
link_cuboid_list = ("arm_base_link_joint_collision_cuboid",
"shoulder_link_collision_cuboid",
"elbow_link_collision_cuboid",
"forearm_link_collision_cuboid",
"wrist_link_collision_cuboid",
"gripper_link_collision_cuboid",
"finger_r_collision_cuboid",
"finger_l_collision_cuboid")
for link_cuboid in link_cuboid_list:
d = vu.get_handle_by_name(clientID, link_cuboid)
link_spec = {}
link_spec["Origin"] = vu.get_object_position(clientID, d)
link_spec["Orientation"] = vu.get_object_orientation(clientID, d)
link_spec["Dimension"] = vu.get_object_bounding_box(clientID, d)
link_cuboid_spec.append(link_spec)
obstacle_cuboid_list = ("cuboid_0", "cuboid_1", "cuboid_2", "cuboid_3",
"cuboid_4", "cuboid_5")
for obstacle_cuboid in obstacle_cuboid_list:
d = vu.get_handle_by_name(clientID, obstacle_cuboid)
obstacle_spec = {}
obstacle_spec["Origin"] = vu.get_object_position(clientID, d)
obstacle_spec["Orientation"] = vu.get_object_orientation(clientID, d)
obstacle_spec["Dimension"] = vu.get_object_bounding_box(clientID, d)
obstacle_cuboid_spec.append(obstacle_spec)
print("done")
joint_targets = get_cuboid_config()
# Instantiate controller
controller = ArmController()
# Iterate through target joint positions
for target in joint_targets:
# Set new target position
controller.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(clientID)
# print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = controller.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
vu.step_sim(clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = controller.has_stably_converged_to_target()
vu.stop_sim(clientID)
# Post simulation cleanup -- save results to a pickle, plot time histories, etc #####
# Fill this out here (optional) or in your own script
# If you use a separate script, don't forget to include it in the deliverables
# ...
#####################################################################################
plt.figure()
for i in range(7):
plt.subplot(2, 4, i + 1)
plt.title('Joint %d' % (i + 1))
plt.xlabel('Time')
plt.ylabel('Joint angle')
a1 = np.array(controller.history['joint_feedback'])[:, i]
a2 = np.array(controller.history['joint_target'])[:, i]
plt.plot(0.05 * np.arange(len(a1)), a1)
plt.plot(0.05 * np.arange(len(a2)), a2)
plt.show()
def execute():
# Connect to V-REP
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
deg_to_rad = np.pi / 180.
# Joint targets. Specify in radians for revolute joints and meters for prismatic joints.
# The order of the targets are as follows:
# joint_1 / revolute / arm_base_link <- shoulder_link
# joint_2 / revolute / shoulder_link <- elbow_link
# joint_3 / revolute / elbow_link <- forearm_link
# joint_4 / revolute / forearm_link <- wrist_link
# joint_5 / revolute / wrist_link <- gripper_link
# joint_6 / prismatic / gripper_link <- finger_r
# joint_7 / prismatic / gripper_link <- finger_l
planned = np.load("execute_path.npy")
res = []
for plan in planned:
res.append(plan.tolist())
joint_targets = res
# Instantiate controller
controller = ArmController()
# Iterate through target joint positions
for i, target in enumerate(joint_targets):
# Set new target position
controller.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(clientID)
#print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = controller.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
#print(sensed_joint_positions)
vu.step_sim(clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = controller.has_stably_converged_to_target()
#visualizer(clientID)
#saveingCuboid(clientID,i)
#savingJoints(clientID,i)
vu.stop_sim(clientID)
print "terminated"
file = open('pickle_history', 'wb')
pickle.dump(controller.history, file)
file.close()
