def scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajectory
return new_traj
def tuck(self):
# prepare a joint trajectory
msg = JointTrajectory()
msg.joint_names = servos
msg.points = list()
point = JointTrajectoryPoint()
point.positions = forward
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(5.0)
msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = to_side
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(8.0)
msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = tucked
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(11.0)
msg.points.append(point)
# call action
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self._client.send_goal(goal)
def dual_arm_test():
pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
rospy.init_node('dual_arm_test')
rospy.sleep(1.0)
while not rospy.is_shutdown():
traj = JointTrajectory()
traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
ptn = JointTrajectoryPoint()
ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.points = []
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
def main():
pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
rospy.init_node('traj_test')
p1 = JointTrajectoryPoint()
p1.positions = [0,0,0,0,0,0,0]
p1.velocities = [0,0,0,0,0,0,0]
p1.accelerations = [0,0,0,0,0,0,0]
p2 = JointTrajectoryPoint()
p2.positions = [0,0,1.0,0,-1.0,0,0]
p2.velocities = [0,0,0,0,0,0,0]
p2.accelerations = [0,0,0,0,0,0,0]
p2.time_from_start = rospy.Time(4.0)
p3 = JointTrajectoryPoint()
p3.positions = [0,0,-1.0,0,1.0,0,0]
p3.velocities = [0,0,0,0,0,0,0]
p3.accelerations = [0,0,0,0,0,0,0]
p3.time_from_start = rospy.Time(20.0)
traj = JointTrajectory()
traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
traj.points = [p1,p2,p3]
rospy.loginfo(traj)
r = rospy.Rate(1) # 10hz
pub.publish(traj)
r.sleep()
pub.publish(traj)
def control_loop(self):
if self.commands is None:
raise Exception('Command list is empty. Was the control plan loaded?')
# loop through the command list
for cmd in self.commands:
# wait for proxy to be in active state
self.wait_for_state(self._proxy_state_running)
# process commands
cmd_spec_str = None
spec = None
t = cmd[ProxyCommand.key_command_type]
if not (t == "noop"):
cmd_spec_str = cmd[ProxyCommand.key_command_spec]
if not isinstance(cmd_spec_str, basestring):
spec = float(cmd_spec_str)
else:
spec = self.positions[cmd_spec_str]
rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec))
# check for any wait depends
self.wait_for_depend(cmd)
# execute command
# could do this with a dictionary-based function lookup, but who cares
if t == 'noop':
rospy.loginfo("Command type: noop")
self.wait_for_depend(cmd)
elif t == 'sleep':
rospy.loginfo("sleep command")
v = float(spec)
rospy.sleep(v)
elif t == 'move_gripper':
rospy.loginfo("gripper command")
self.move_gripper(spec)
elif t == 'move_arm':
rospy.loginfo("move_arm command")
rospy.logdebug(spec)
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self.arm_joint_names
jtp = JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(0.5) # fudge factor for gazebo controller
jtp.positions = spec
jtp.velocities = [0]*len(spec)
goal.trajectory.points.append(jtp)
self._arm_goal = copy.deepcopy(goal)
self.move_arm()
elif t == 'plan_exec_arm':
rospy.loginfo("plan and execute command not implemented")
raise NotImplementedError()
else:
raise Exception("Invalid command type: " + str(cmd.type))
# check for any set dependencies action
self.set_depend(cmd)
# check for any clear dependencies action
self.clear_depend(cmd)
def __init__(self, arm = 'l', record_data = False):
self.arm = arm
if arm == 'r':
print "using right arm on Darci"
self.RIGHT_ARM = 0 # i don't think this was used anywhere
else:
self.LEFT_ARM = 1
# print "Left and both arms not implemented in this client yet ... \n"
# print "will require writing different function calls since joint data for left and right arm come in together from the meka server"
# sys.exit()
self.kinematics = dak.DarciArmKinematics(arm)
self.joint_pub = rospy.Publisher('/'+self.arm+'_arm_controller/command', JointTrajectory)
self.joint_names = None
self.lock = RLock()
self.joint_angles = None
self.joint_velocities = None
self.J_h = None
self.time = None
self.desired_joint_angles = None
self.stiffness_percent = 0.75
self.ee_force = None
self.ee_torque = None
self.skins = None #will need code for all of these skin interfaces
self.Jc_l = []
self.n_l = []
self.values_l = []
#values from m3gtt repository in robot_config folder
# These could be read in from a yaml file like this (import yaml; stream = open("FILE_NAME_HERE", 'r'); data = yaml.load(stream))
# However, not clear if absolute path to yaml file (possibly on another computer) is better then just defining it here
# The downside is obviously if someone tunes gains differently on the robot.
self.joint_stiffness = (np.array([1, 1, 1, 1, 0.06, 0.08, 0.08])*180/np.pi*self.stiffness_percent).tolist()
self.joint_damping = (np.array([0.06, 0.1, 0.015, 0.015, 0.0015, 0.002, 0.002])*180/np.pi*self.stiffness_percent).tolist()
self.record_data = record_data
if self.record_data:
from collections import deque
self.q_record = deque()
self.qd_record = deque()
self.times = deque()
self.state_sub = rospy.Subscriber('/'+self.arm+'_arm_controller/state', JointTrajectoryControllerState, self.robotStateCallback)
rospy.sleep(1.0)
while self.joint_angles is None:
rospy.sleep(0.05)
self.desired_joint_angles = copy.copy(self.joint_angles)
self.joint_cmd = JointTrajectory()
self.joint_cmd.header.stamp = rospy.Time.now()
self.joint_cmd.header.frame_id = '/torso_lift_link'
self.joint_cmd.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = self.desired_joint_angles
jtp.velocities = [1.]*len(self.joint_names)
self.joint_cmd.points = [jtp]
self.joint_pub.publish(self.joint_cmd)
def traj_speed_up(traj, spd=3.0):
new_traj = RobotTrajectory()
new_traj = traj
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * spd
point.accelerations[j] = point.accelerations[j] * spd * spd
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def test_5(self):
# Test 5: move each joint in robotic arm to a specific value at the same time (mixed speeds)
rospy.loginfo("Begin test 5")
# Create a joint state publisher
self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory)
rospy.sleep(1)
# How fast will we update the robot's movement? in Hz
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Define a joint trajectory message for grasp position
rospy.loginfo("Position arm in stowed position")
msg = JointTrajectory()
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '0'
msg.joint_names.append('joint_1')
msg.joint_names.append('joint_2')
msg.joint_names.append('joint_3')
msg.joint_names.append('joint_4')
msg.joint_names.append('joint_5')
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, -1.3, 1.7, 1.2, 0.1]
point.velocities = [90.0, 50.0, 20.0, 10.0, 10.0]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
# publish joint stage message
self.arm_pub.publish(msg)
rospy.loginfo("End test 5")
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
if self._mode == "velocity":
velocities = [0.0] * len(joint_names)
cmd = dict(zip(joint_names, velocities))
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_velocities(cmd)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
elif self._mode == "position" or self._mode == "position_w_id":
raw_pos_mode = self._mode == "position_w_id"
if raw_pos_mode:
pnt = JointTrajectoryPoint()
pnt.positions = self._get_current_position(joint_names)
if dimensions_dict["velocities"]:
pnt.velocities = [0.0] * len(joint_names)
if dimensions_dict["accelerations"]:
pnt.accelerations = [0.0] * len(joint_names)
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
# zero inverse dynamics feedforward command
if self._mode == "position_w_id":
pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
self._pub_ff_cmd.publish(ff_pnt)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
def add_point(self, positions, velocities, accelerations, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = copy(velocities)
point.accelerations = copy(accelerations)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def build_traj(start, end, duration):
if sorted(start.name) <> sorted(end.name):
rospy.logerr('Start and End position joint_names mismatch')
raise
# assume the start-position joint-ordering
joint_names = start.name
# 始点定義
start_pt = JointTrajectoryPoint()
start_pt.positions = start.position
start_pt.velocities = [0]*len(start.position)
start_pt.time_from_start = rospy.Duration(0.0) # 始点なので、待ち時間 = 0 [sec]
# 中間地点定義
middle_pt = JointTrajectoryPoint()
for j in joint_names:
idx = end.name.index(j)
middle_pt.positions.append((start.position[idx] + end.position[idx])/2.0)
middle_pt.velocities.append(0)
middle_pt.time_from_start = rospy.Duration(duration) # 中間地点までの到達時間 = duration [sec]
# 執着地点
end_pt = JointTrajectoryPoint()
for j in joint_names:
idx = end.name.index(j)
end_pt.positions.append(end.position[idx])
end_pt.velocities.append(0.0)
end_pt.time_from_start = rospy.Duration(duration*2) # 終点までの到達時間 = duration*2 [sec]
return JointTrajectory(joint_names=joint_names, points=[start_pt, middle_pt, end_pt])
def build_traj(start, end, duration):
# print start.name
# print end.name
if sorted(start.name) <> sorted(end.name):
rospy.logerr('Start and End position joint_names mismatch')
raise
# assume the start-position joint-ordering
joint_names = start.name
start_pt = JointTrajectoryPoint()
start_pt.positions = start.position
start_pt.velocities = [0.05]*len(start.position)
start_pt.time_from_start = rospy.Duration(0.0)
end_pt = JointTrajectoryPoint()
# end_pt = start_pt;
# end_pt.positions[3] = end_pt.positions[3]+.1
for j in joint_names:
idx = end.name.index(j)
end_pt.positions.append(end.position[idx]) # reorder to match start-pos joint ordering
end_pt.velocities.append(0)
end_pt.time_from_start = rospy.Duration(duration)
return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"):
"""
This function will convert the plan to a joint trajectory compatible with the
/arm_N/arm_controller/command topic
"""
theplan = plan
# create joint trajectory message
jt = JointTrajectory()
# fill the header
jt.header.seq = seq
jt.header.stamp.secs = 0 # secs
jt.header.stamp.nsecs = 0 # nsecs
jt.header.frame_id = frame_id
# specify the joint names
jt.joint_names = theplan.joint_trajectory.joint_names
# fill the trajectory points and computer constraint times
njtp = len(theplan.joint_trajectory.points)
for ii in range(0, njtp):
jtp = JointTrajectoryPoint()
jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1))
jt.points.append(jtp)
return jt
def check_collision(self):
if not self.collsion_srv:
return False
# Create a new trajectory by combining both the trajectories, assumption is that the timing is same
# The order of joining should be [left + right]
self.adjust_traj_length()
traj = JointTrajectory()
traj.joint_names = self.left._goal.trajectory.joint_names + self.right._goal.trajectory.joint_names
no_points = len(self.left._goal.trajectory.points)
for index in xrange(no_points):
pt = JointTrajectoryPoint()
left_pt = self.left._goal.trajectory.points[index]
right_pt = self.right._goal.trajectory.points[index]
pt.positions = left_pt.positions + right_pt.positions
pt.velocities = left_pt.velocities + right_pt.velocities
pt.time_from_start = left_pt.time_from_start
traj.points.append(pt)
msg = CollisionCheckRequest()
msg.trajectory = traj
try:
collision_service = rospy.ServiceProxy("collision_check", CollisionCheck)
resp = collision_service(msg)
if resp.collision_found:
rospy.logwarn("Collision Found")
else:
rospy.loginfo("Collision not found, good to go :)")
return resp.collision_found
except rospy.ServiceException as e:
rospy.logwarn("Exception on collision check {}".format(e))
return True
def runTrajectory(req):
global Traj_server;
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
else:
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);
if req.task == "Forward":
file_name = file_root+"/forward";
elif req.task == "Drop":
file_name = file_root+"/drop";
else :
return taskResponse(False);
f = open(file_name,"r");
plan_file = RobotTrajectory();
buf = f.read();
plan_file.deserialize(buf);
plan = copy.deepcopy(plan_file);
arm_right_group = moveit_commander.MoveGroupCommander("arm_right");
arm_left_group = moveit_commander.MoveGroupCommander("arm_left");
arm_left_group.set_start_state_to_current_state();
StartPnt = JointTrajectoryPoint();
StartPnt.positions = arm_left_group.get_current_joint_values();
StartPnt.velocities = [0]*len(StartPnt.positions);
StartPnt. accelerations = [0]*len(StartPnt.positions);
#print StartPnt;
plan.joint_trajectory.points[0] = StartPnt;
#print plan;
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory();
robot = moveit_commander.RobotCommander();
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while", file_name, " is visualized (again)..."
arm_left_group.execute(plan);
print "Trajectory ", file_name, " finished!"
f.close();
return taskResponse(True);
def stage_3(self):
# Move arm to grasping position
rospy.loginfo("Begin stage 3")
# Create a joint state publisher
self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory)
rospy.sleep(1)
# How fast will we update the robot's movement? in Hz
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Define a joint trajectory message for grasp position
rospy.loginfo("Position arm in grasp position")
msg = JointTrajectory()
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '0'
msg.joint_names.append('joint_1')
msg.joint_names.append('joint_2')
msg.joint_names.append('joint_3')
msg.joint_names.append('joint_4')
msg.joint_names.append('joint_5')
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, 0.0, 0.0, 1.6, 0.1]
point.velocities = [20.0, 20.0, 20.0, 10.0, 10.0]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
# publish joint stage message
self.arm_pub.publish(msg)
rospy.sleep(3)
def test_trajectory(self):
# our goal variable
goal = FollowJointTrajectoryGoal()
# First, the joint names, which apply to all waypoints
goal.trajectory.joint_names = ('shoulder_pitch_joint',
'shoulder_pan_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_pitch_joint',
'wrist_roll_joint')
# We will have two waypoints in this goal trajectory
# First trajectory point
# To be reached 1 second after starting along the trajectory
point = JointTrajectoryPoint()
point.positions = [0.0] * 7
point.velocities = [1.0] * 7
point.time_from_start = rospy.Duration(3.0)
goal.trajectory.points.append(point)
#we are done; return the goal
return goal
def talker():
if 1 == 0:
pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"]
jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"]
else:
pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
jn = [
"LeftShoulderPitch",
"LeftShoulderRoll",
"LeftShoulderYaw",
"LeftElbowPitch",
"LeftForearmYaw",
"LeftWristRoll",
"LeftWristPitch",
]
jn_r = [
"RightShoulderPitch",
"RightShoulderRoll",
"RightShoulderYaw",
"RightElbowPitch",
"RightForearmYaw",
"RightWristRoll",
"RightWristPitch",
]
# this doesnt work:
# jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"]
# value = 0
rospy.init_node("send_arm_test", anonymous=True)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
msg = JointTrajectory()
value = 0.1
value_r = 0.1
msg.joint_names = jn
pt = JointTrajectoryPoint()
pt.positions = [value] * len(jn)
pt.velocities = [0] * len(jn)
pt.accelerations = [0] * len(jn)
pt.effort = [0] * len(jn)
pt.time_from_start = rospy.Duration.from_sec(3)
msg.points.append(pt)
print msg.joint_names, rospy.get_time()
pub.publish(msg)
# TODO: add a sleep here between left and right
msg.joint_names = jn_r
msg.points[0].positions = [value_r] * len(jn)
print msg.joint_names, rospy.get_time()
pub.publish(msg)
rate.sleep()
def up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [1.]*len(self.joint_names)
jtp.velocities = [0.]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(5.0)
jtm.points = [jtp]
return jtm
def random_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = 2*np.random.random(len(self.joint_names)) - 1
jtp.velocities = [0.]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(5.)
jtm.points = [jtp]
return jtm
def test_controller(self):
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
topic = "/pr2/torso_controller/command"
rospy.init_node("test_pr2_torso_controller", log_level=rospy.DEBUG, disable_signals=True)
rospy.loginfo("Preparing to publish on %s" % topic)
ctl = rospy.Publisher(topic, JointTrajectory)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
duration = 0.1
traj = JointTrajectory()
traj.joint_name = "torso_lift_joint"
initialpoint = JointTrajectoryPoint()
initialpoint.positions = 0.0
initialpoint.velocities = 0.0
initialpoint.time_from_start = rospy.Duration(0.0)
finalpoint = JointTrajectoryPoint()
finalpoint.positions = 0.195
finalpoint.velocities = 0.0
finalpoint.time_from_start = rospy.Duration(duration)
# First, move up
traj.points = [initialpoint, finalpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.195)
# Go back to initial position
finalpoint.time_from_start = rospy.Duration(0.0)
initialpoint.time_from_start = rospy.Duration(duration)
traj.points = [finalpoint, initialpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
def set_position(self,goal): traj = JointTrajectory() traj_p = JointTrajectoryPoint() traj.joint_names = ["torso_lift_joint"] traj_p.positions = [goal] traj_p.velocities = [0.] traj_p.accelerations = [0.] traj_p.time_from_start = rospy.Duration(2.) traj.points.append(traj_p) self.pub.publish(traj)
def make_joint_trajectory(names, points):
jt = JointTrajectory()
jt.joint_names = names
pt = JointTrajectoryPoint()
pt.positions = points
pt.effort = [0]*len(points)
pt.velocities = [0]*len(points)
pt.accelerations = [0]*len(points)
jt.points = [pt]
return jt
def np(pts, pos, time):
pt = JointTrajectoryPoint()
pt.positions = [pos]
if len(pts)==0:
st = rospy.Duration(0)
else:
st = pts[-1].time_from_start
pt.velocities = [0]
pt.time_from_start = st + rospy.Duration(time)
pts.append(pt)
def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None):
# Hack
vel_arr = [[0.]*7]*len(pos_arr)
acc_arr = [[0.]*7]*len(pos_arr)
##
# Compute joint velocities and acclereations.
def get_vel_acc(q_arr, d_arr):
vel_arr = [[0.]*7]
acc_arr = [[0.]*7]
for i in range(1, len(q_arr)):
vel, acc = [], []
for j in range(7):
vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]]
acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]]
vel_arr += [vel]
acc_arr += [acc]
print vel, acc
return vel_arr, acc_arr
if arm != 1:
arm = 0
jtg = JointTrajectoryGoal()
if stamp is None:
stamp = rospy.Time.now()
else:
jtg.trajectory.header.stamp = stamp
if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
if vel_arr is None:
vel_arr = v_arr
if acc_arr is None:
acc_arr = a_arr
jtg.trajectory.joint_names = self.joint_names_list[arm]
for i in range(len(pos_arr)):
if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
continue
jtp = JointTrajectoryPoint()
jtp.positions = pos_arr[i]
if vel_arr is None:
vel = [0.] * 7
else:
vel = vel_arr[i]
jtp.velocities = vel
if acc_arr is None:
acc = [0.] * 7
else:
acc = acc_arr[i]
jtp.accelerations = acc
jtp.time_from_start = rospy.Duration(dur_arr[i])
jtg.trajectory.points.append(jtp)
return jtg
def stage_8(self):
# move arms to stowed position
rospy.loginfo("Begin stage 8")
# Stow robotic arm
# Create a joint state publisher for each turtlebot
self.arm_1_pub = rospy.Publisher('turtlebot_1/arm_controller/command', JointTrajectory)
self.arm_2_pub = rospy.Publisher('turtlebot_2/arm_controller/command', JointTrajectory)
rospy.sleep(1)
# How fast will we update the robot's movement? in Hz
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Define a joint trajectory message for grasp position
rospy.loginfo("Position arm in stowed position")
rospy.sleep(3)
msg = JointTrajectory()
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '0'
msg.joint_names.append('joint_1')
msg.joint_names.append('joint_2')
msg.joint_names.append('joint_3')
msg.joint_names.append('joint_4')
msg.joint_names.append('joint_5')
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, 1.5, 0.7, 0.0, -1.8]
point.velocities = [2.0, 2.0, 20.0, 10.0, 1.0]
point.time_from_start = rospy.Duration(2.0)
msg.points.append(point)
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, -1.0, 1.9, 1.2, 0.1]
point.velocities = [50.0, 50.0, 50.0, 50.0, 10.0]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
# publish joint stage message
self.arm_1_pub.publish(msg)
self.arm_2_pub.publish(msg)
rospy.sleep(3)
def move_gripper(self, opening_mm):
# Creates a goal to send to the action server.
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self.gripper_joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [opening_mm/2000.0, opening_mm/2000.0]
jtp.velocities = [0, 0]
jtp.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(jtp)
self._ac_gripper.send_goal(goal)
self._ac_gripper.wait_for_result()
return self._ac_gripper.get_result()
def init_left_arm_cb(userdata, goal): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(2) point.positions = [ -0.5, 0.1, -0.1, 0.6109, 0.0, 0.0, 0.0 ] point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] joint_goal = JointTrajectoryGoal() joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'] joint_goal.trajectory.header.stamp = rospy.get_rostime() joint_goal.trajectory.points.append(point) return joint_goal
def tuck_right_arm_torso_cb(userdata,goal): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(2) point.positions = [ 0.0, 0.0, 0.3, 0.1, -1.0, 0.8, 0.0, 0.0, 0.0 ] point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] joint_goal = JointTrajectoryGoal() joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint'] joint_goal.trajectory.header.stamp = rospy.get_rostime() joint_goal.trajectory.points.append(point) return joint_goal
def robotTrajectoryFromPlan(self, plan, joint_names):
"""Given a dmp plan (GetDMPPlanResponse) create a RobotTrajectory to be able to visualize what it consists and also
to be able to send it to execution"""
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = joint_names
for point, time in zip(plan.plan.points, plan.plan.times):
jtp = JointTrajectoryPoint()
jtp.positions = point.positions
jtp.velocities = point.velocities
jtp.time_from_start = rospy.Duration( time )
#rospy.logwarn("jtp is: " + str(jtp))
rt.joint_trajectory.points.append(jtp)
return rt
def point():
rospy.init_node('point_control')
pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=10)
rospy.Subscriber('/arm_start_stop', Bool, run_cb)
rospy.Subscriber("/offset_joint_azim", UInt16, offset_joint_azim_cb)
rospy.Subscriber("/offset_joint_elev", UInt16, offset_joint_elev_cb)
rospy.Subscriber("/vel_1", UInt16, vel_1_cb)
rospy.Subscriber("/vel_2", UInt16, vel_2_cb)
grados_joint_azim = rospy.get_param("/grados_joint_azim")
rospy.loginfo("%s is %s", rospy.resolve_name('grados_joint_azim'),
grados_joint_azim)
grados_joint_elev = rospy.get_param("/grados_joint_elev")
rospy.loginfo("%s is %s", rospy.resolve_name("/grados_joint_elev"),
grados_joint_elev)
ang1 = grados_joint_azim * np.pi / 180 #np.pi*2
ang2 = grados_joint_elev * np.pi / 180 #np.pi*3/4
offset_joint_azim = rospy.get_param("/offset_joint_azim")
rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_azim"),
offset_joint_azim)
offset_joint_elev = rospy.get_param("/offset_joint_elev")
rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_elev"),
offset_joint_elev)
ang1_offset = offset_joint_azim * np.pi / 180 #np.pi*2
ang2_offset = offset_joint_elev * np.pi / 180 #np.pi*3/4
pub_period = 0.05
vel_1 = rospy.get_param("/vel_1")
rospy.loginfo("%s is %s", rospy.resolve_name('/vel_1'), vel_1)
vel_2 = rospy.get_param("/vel_2")
rospy.loginfo("%s is %s", rospy.resolve_name('/vel_2'), vel_2)
start_time = time.time()
time.sleep(0.01)
next_point_time = 0
dt = 0
while not rospy.is_shutdown():
if (time.time() > next_point_time) and run_var == True:
dt = time.time() - start_time
p = JointTrajectoryPoint()
i = ang1 * np.sin((vel_1 * dt) * 2 * np.pi) + ang1_offset
j = ang2 * np.sin((vel_2 * dt) * 2 * np.pi) + ang2_offset
k = vel_1 * 2 * np.pi * ang1 * np.cos(
(vel_1 * (dt - pub_period)) * 2 * np.pi)
l = vel_2 * 2 * np.pi * ang2 * np.cos((vel_2 * dt) * 2 * np.pi)
p.positions = [i, j]
p.velocities = [k, l]
p.accelerations = [0, 0]
p.time_from_start = rospy.Time.now()
rospy.loginfo("point \n %s ", p)
pub.publish(p)
next_point_time = time.time() + pub_period
elif run_var == False:
start_time = time.time() + dt
def main():
step_ts = 0.004
rospy.init_node("test_moveit_commander_custom_trajectory", anonymous=True)
rospy.Subscriber("controller_state", ControllerState, callback_function)
robot = urdf.robot_from_parameter_server()
controller_commander=controller_commander_pkg.ControllerCommander()
controller_commander.set_controller_mode(controller_commander.MODE_AUTO_TRAJECTORY, 0.4, [], [])
time.sleep(0.5)
tran = np.array([2.197026484647054, 1.2179574262842452, 0.12376598588449844])
rot = np.array([[-0.99804142, 0.00642963, 0.06222524], [ 0.00583933, 0.99993626, -0.00966372], [-0.06228341, -0.00928144, -0.99801535]])
pose_target2 = rox.Transform(rot, tran)
pose_target2.p[2] += 0.20
print 'Target:',pose_target2
print "============ Move Close to Panel"
controller_commander.compute_cartesian_path_and_move(pose_target2, avoid_collisions=False)
controller_commander.set_controller_mode(controller_commander.MODE_AUTO_TRAJECTORY, 1.0, [], [])
time.sleep(1.0)
Kc = 0.004
time_save = []
FTdata_save = []
Tran_z = np.array([[0,0,-1],[0,-1,0],[1,0,0]])
Vec_wrench = 100*np.array([0.019296738361905,0.056232033265447,0.088644197659430,
0.620524934626544,-0.517896661195076,0.279323567303444,-0.059640563813256,
0.631460085138371,-0.151143175570223,-6.018321330845553]).transpose()
listener=PayloadTransformListener()
rapid_node = rapid_node_pkg.RAPIDCommander()
#controller_commander=controller_commander_pkg.arm_composites_manufacturing_controller_commander()
time.sleep(1.0)
FTdata_0 = FTdata
T = listener.lookupTransform("base", "link_6", rospy.Time(0))
rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose())
A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])])
A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)])
A = np.vstack([A1,A2])
FTdata_0est = np.matmul(A,Vec_wrench)
#print 'Test4:',controller_commander.ControllerState
for i in range(400):
tic = time.time()
plan=RobotTrajectory()
plan.joint_trajectory.joint_names=['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
current_joint_angles = controller_commander.get_current_joint_values()
plan.joint_trajectory.header.frame_id='/world'
p1=JointTrajectoryPoint()
p1.positions = current_joint_angles
p1.velocities = np.zeros((6,))
p1.accelerations = np.zeros((6,))
p1.time_from_start = rospy.Duration(0)
T = listener.lookupTransform("base", "link_6", rospy.Time(0))
rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose())
A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])])
A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)])
A = np.vstack([A1,A2])
FTdata_est = np.matmul(A,Vec_wrench)
#print 'Test0:',FTdata,FTdata_0,FTdata_est,FTdata_0est
FTread = FTdata-FTdata_0-FTdata_est+FTdata_0est
print 'FTread:',FTread
print 'FT:',FTdata
print 'Z',FTread[-1]
if FTread[-1]>-200:
F_d = -350
else:
F_d = -400
J = rox.robotjacobian(robot, current_joint_angles)
Vz = Kc*(F_d - FTread[-1])
joints_vel = np.linalg.pinv(J).dot(np.array([0,0,0,0,0,Vz]+dx))
print 'Joint_command:',joints_vel.dot(step_ts)
p2=JointTrajectoryPoint()
p2.positions = np.array(p1.positions) + joints_vel.dot(step_ts)#np.array([0,np.deg2rad(-2),0,0,0,0])
p2.velocities = np.zeros((6,))
p2.accelerations = np.zeros((6,))
p2.time_from_start = rospy.Duration(step_ts)
plan.joint_trajectory.points.append(p1)
plan.joint_trajectory.points.append(p2)
controller_commander.execute(plan)
print 'Time:', time.time()-tic
time_save.append(time.time())
FTdata_save.append(FTread)
filename = "FTdata.txt"
f_handle = file(filename, 'a')
np.savetxt(f_handle, FTdata_save)
f_handle.close()
filename = "Time.txt"
f_handle = file(filename, 'a')
np.savetxt(f_handle, time_save)
f_handle.close()
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name, ))
self._server.set_aborted()
return
rospy.loginfo("%s: Executing requested joint trajectory" %
(self._action_name, ))
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(
joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
if dimensions_dict['velocities']:
first_trajectory_point.velocities = deepcopy(
trajectory_points[0].velocities)
if dimensions_dict['accelerations']:
first_trajectory_point.accelerations = deepcopy(
trajectory_points[0].accelerations)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if dimensions_dict['velocities']:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 6 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
except Exception as ex:
rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
" arm with error \"{2}: {3}\"").format(
self._action_name, self._name,
type(ex).__name__, ex))
self._server.set_aborted()
return
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
crustcrawler_dataflow.wait_for(lambda: rospy.get_time() >= start_time,
timeout=float('inf'))
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while (now_from_start < end_time and not rospy.is_shutdown()):
#Acquire Mutex
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
#Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = (now_from_start - pnt_times[idx - 1])
t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
else:
cmd_time = 0
t = 0
point = self._get_bezier_point(b_matrix, idx, t, cmd_time,
dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point,
start_time,
dimensions_dict)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
# Release the Mutex
if not command_executed:
return
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
if (self._stopped_velocity > 0.0 and max([
abs(cur_vel)
for cur_vel in self._get_current_velocities(joint_names)
]) > self._stopped_velocity):
return False
else:
return True
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown()):
if not self._command_joints(joint_names, last, start_time,
dimensions_dict):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
# Verify goal constraint
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr(
"%s: Exceeded Max Goal Velocity Threshold for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" %
(self._action_name, result, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop(goal.trajectory.joint_names, end_angles, start_time,
dimensions_dict)
rospy.wait_for_service('/ve026a/set_motor')
try:
res = rospy.ServiceProxy('/ve026a/set_motor', SetBool)(True)
print(res.message)
except rospy.ServiceException, e:
print("Service call failed, %s" % e)
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
p1 = JointTrajectoryPoint()
p1.positions = [0, 0, 0, 0, 0, 0]
p1.velocities = [0, 0, 0, 0, 0, 0]
p1.time_from_start = rospy.Duration.from_sec(3.0)
p2 = JointTrajectoryPoint()
p2.positions = [0.5, 0.5, 0, 0, 0, 0]
p2.velocities = [0, 0, 0, 0, 0, 0]
p2.time_from_start = rospy.Duration.from_sec(5.0)
p3 = JointTrajectoryPoint()
p3.positions = [0, 0, 0, 0, 0, 0]
p3.velocities = [0, 0, 0, 0, 0, 0]
p3.time_from_start = rospy.Duration.from_sec(7.0)
msg.points = [p1, p2, p3]
r = rospy.Rate(10)
def move_arm(self, points_list):
# # Unpause the physics
# rospy.wait_for_service('/gazebo/unpause_physics')
# unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
# unpause_gazebo()
self.client.wait_for_server()
goal = FollowJointTrajectoryGoal()
# We need to fill the goal message with its components
#
# check msg structure with: rosmsg info FollowJointTrajectoryGoal
# It is composed of 4 sub-messages:
# "trajectory" of type trajectory_msgs/JointTrajectory
# "path_tolerance" of type control_msgs/JointTolerance
# "goal_tolerance" of type control_msgs/JointTolerance
# "goal_time_tolerance" of type duration
trajectory_msg = JointTrajectory()
# check msg structure with: rosmsg info JointTrajectory
# It is composed of 3 sub-messages:
# "header" of type std_msgs/Header
# "joint_names" of type string
# "points" of type trajectory_msgs/JointTrajectoryPoint
trajectory_msg.joint_names = [
"j2n6s300_joint_1",
"j2n6s300_joint_2",
"j2n6s300_joint_3",
"j2n6s300_joint_4",
"j2n6s300_joint_5",
"j2n6s300_joint_6"
]
points_msg = JointTrajectoryPoint()
# check msg structure with: rosmsg info JointTrajectoryPoint
# It is composed of 5 sub-messages:
# "positions" of type float64
# "velocities" of type float64
# "accelerations" of type float64
# "efforts" of type float64
# "time_from_start" of type duration
points_msg.positions = points_list
points_msg.velocities = [0, 0, 0, 0, 0, 0]
points_msg.accelerations = [0, 0, 0, 0, 0, 0]
points_msg.effort = [0, 0, 0, 0, 0, 0]
points_msg.time_from_start = rospy.Duration(0.01)
# fill in points message of the trajectory message
trajectory_msg.points = [points_msg]
# fill in trajectory message of the goal
goal.trajectory = trajectory_msg
# self.client.send_goal_and_wait(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
# wait for the robot to finish moving
# rospy.sleep(2) # wait for 2s
while not self.robot_has_stopped():
pass
def interpolated_ik_motion_planner_callback(self, req):
#names and angles for the joints in their desired order
joint_names = req.motion_plan_request.start_state.joint_state.name
start_angles = req.motion_plan_request.start_state.joint_state.position
#sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified
if start_angles and len(joint_names) != len(start_angles):
rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting")
return 0
#reorder the start angles to the order needed by IK
reordered_start_angles = []
#get the current joint states for the robot
#joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0)
#if not joint_states_msg:
# rospy.logerr("unable to get joint_states message")
# return 0
#get the desired start angles for each IK arm joint in turn from start_state.joint_state.position
#(use the current angle if not specified)
for joint_name in self.ik_utils.joint_names:
#desired start angle specified
if joint_name in joint_names and start_angles:
index = joint_names.index(joint_name)
reordered_start_angles.append(start_angles[index])
else:
rospy.logerr("missing joint angle, can't deal")
return 0
#desired start angle not specified, use the current angle
#elif 0: #joint_name in joint_states_msg.name:
# index = joint_states_msg.name.index(joint_name)
# current_position = joint_states_msg.position[index]
# reordered_start_angles.append(current_position)
#malformed joint_states message?
# else:
# rospy.logerr("an expected arm joint,"+joint_name+"was not found!")
# return 0
#get additional desired joint angles (such as for the gripper) to pass through to IK
additional_joint_angles = []
additional_joint_names = []
for (ind, joint_name) in enumerate(joint_names):
if joint_name not in self.ik_utils.joint_names:
#rospy.loginfo("found %s"%joint_name)
additional_joint_angles.append(start_angles[ind])
additional_joint_names.append(joint_name)
IK_robot_state = None
if additional_joint_angles:
#rospy.loginfo("adding additional start angles for:"+str(additional_joint_names))
#rospy.loginfo("additional joint angles:"+str(additional_joint_angles))
IK_robot_state = RobotState()
IK_robot_state.joint_state.name = additional_joint_names
IK_robot_state.joint_state.position = additional_joint_angles
#check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now)
link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0]
if link_name != self.ik_utils.link_name:
rospy.logerr("link_name not allowed: %s"%link_name)
return 0
#the start pose for that link
start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0]
#the frame that start pose is in
frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0]
#turn it into a PoseStamped
start_pose_stamped = self.add_header(PoseStamped(), frame_id)
start_pose_stamped.pose = start_pose
#the desired goal position
goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position
#the frame that goal position is in
goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id
#convert the position to base_link frame
goal_ps = self.add_header(PointStamped(), goal_pos_frame)
goal_ps.point = goal_pos
goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, 'base_link')
#the desired goal orientation
goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation
#the frame that goal orientation is in
goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id
#convert the quaternion to base_link frame
goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
goal_qs.quaternion = goal_quat
goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, 'base_link')
#assemble the goal pose into a PoseStamped
goal_pose_stamped = self.add_header(PoseStamped(), 'base_link')
goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list))
#get the ordered collision operations, if there are any
ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations
#if ordered_collision_operations.collision_operations == []:
# ordered_collision_operations = None
#get the link paddings, if there are any
link_padding = None #req.motion_plan_request.link_padding
#if link_padding == []:
# link_padding = None
#RUN! Check the Cartesian path for consistent, non-colliding IK solutions
(trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
self.steps_before_abort, self.num_steps, ordered_collision_operations, \
self.start_from_end, IK_robot_state, link_padding)
#find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0)
#if we're searching from the end, keep the end; if we're searching from the start, keep the start
start_ind = 0
stop_ind = len(error_codes)
if self.start_from_end:
for ind in range(len(error_codes)-1, 0, -1):
if error_codes[ind]:
start_ind = ind+1
break
else:
for ind in range(len(error_codes)):
if error_codes[ind]:
stop_ind = ind
break
(times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs)
times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind)
vels = [[0]*7]*start_ind + vels + [[0]*7]*(len(error_codes)-stop_ind)
rospy.logdebug("trajectory:")
for ind in range(len(trajectory)):
rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
rospy.logdebug("")
for ind in range(len(trajectory)):
rospy.logdebug("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
#the response
res = GetMotionPlanResponse()
#the arm joint names in the normal order, as spit out by IKQuery
res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]
#a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal
#(all 0s if there was no IK solution for a point on the path)
res.trajectory.joint_trajectory.points = []
for i in range(len(trajectory)):
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = trajectory[i]
joint_trajectory_point.velocities = vels[i]
joint_trajectory_point.time_from_start = rospy.Duration(times[i])
res.trajectory.joint_trajectory.points.append(joint_trajectory_point)
#a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows:
#ArmNavigationErrorCodes.SUCCESS (1): no problem
#ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
#ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point
#ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
#ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point
error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
4:ArmNavigationErrorCodes.PLANNING_FAILED}
trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes]
res.trajectory_error_codes = trajectory_error_codes
res.error_code.val = ArmNavigationErrorCodes.SUCCESS
# rospy.loginfo("trajectory:")
# for ind in range(len(trajectory)):
# rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
# rospy.loginfo("")
# for ind in range(len(trajectory)):
# rospy.loginfo("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
return res
d = rospy.Duration
while not rospy.is_shutdown():
fake_traj = FollowJointTrajectoryActionGoal()
fake_traj.header = Header()
# fake_traj.header.stamp
fake_traj.goal = FollowJointTrajectoryGoal()
fake_traj.goal.trajectory = JointTrajectory()
fake_traj.goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5'
]
# fake_traj.goal.trajectory.points = JointTrajectoryPoint()[]
for j in xrange(20):
point = JointTrajectoryPoint()
point.positions = [random.random() for i in xrange(5)]
point.velocities = [random.random() for i in xrange(5)]
point.time_from_start = rospy.Duration.from_sec(j)
fake_traj.goal.trajectory.points.append(point)
# fake_traj.goal.trajectory.points.positions = [[random.random() ]
# fake_traj.goal.trajectory.points.velocities = [[random.random() for i in xrange(5)] for j in xrange(20)]
# fake_traj.goal.trajectory.points.time_from_start = [d.from_sec(i * 0.1) for i in xrange(20)]
# fake_traj.goal.trajectory.points.append(positions)
# fake_traj.goal.trajectory.points.append(velocities)
# fake_traj.goal.trajectory.points.append(time_from_start)
pub.publish(fake_traj)
rate.sleep()
g.command = goal
gms.goal=g
# gp.publish(gms)
msg = JointTrajectory()
subMsg = JointTrajectoryPoint()
# print(msg)
# print(subMsg)
# print(type(zeros))
# solver.printDoubleVec(q_targ)
print(ths)
subMsg.positions = ths
subMsg.velocities = zeros
subMsg.accelerations = zeros
msg.joint_names = joints
msg.points=[subMsg]
rate=rospy.Rate(1)
subMsg.time_from_start = rospy.Duration(0.5)
for i in range(2):
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
gp.publish(gms)
rate.sleep()
from actionlib import ActionClient
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
rospy.init_node('initial_joint_state_setter', anonymous=True)
ac = ActionClient('/whole_body_controller/body/follow_joint_trajectory',
FollowJointTrajectoryAction)
ac.wait_for_server()
goal = FollowJointTrajectoryGoal()
traj = JointTrajectory()
traj.header.stamp = rospy.get_rostime()
traj.joint_names = [
'torso_lift_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint',
'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint',
'r_wrist_flex_joint', 'r_wrist_roll_joint', 'l_upper_arm_roll_joint',
'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint',
'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint',
'head_pan_joint', 'head_tilt_joint'
]
p = JointTrajectoryPoint()
p.positions = [
0.1687062500000004, 0, 0, 0, 0, -1.1400000000000001, -1.0499999999999998,
0, 0, 0, 0, 0, -1.1399999999999995, -1.0499999999999998, 0, 0, 0
]
p.velocities = [0] * len(p.positions)
p.time_from_start = rospy.Duration(1)
traj.points.append(p)
goal.trajectory = traj
ac.send_goal(goal)
rospy.init_node('send_open_manipulator_gripper_joint_angles')
pub = rospy.Publisher('/gripper_controller/command',
JointTrajectory,
queue_size=1)
controller_name = "gripper_controller"
joint_names = rospy.get_param("/%s/joints" % controller_name)
rospy.loginfo("Joint names: %s" % joint_names)
rate = rospy.Rate(10)
trajectory_command = JointTrajectory()
trajectory_command.joint_names = joint_names
point = JointTrajectoryPoint()
#Joint names: ['gripper']
point.positions = [-0.1]
point.velocities = [0.0]
point.time_from_start = rospy.rostime.Duration(1, 0)
trajectory_command.points = [point]
while not rospy.is_shutdown():
trajectory_command.header.stamp = rospy.Time.now()
pub.publish(trajectory_command)
rate.sleep()
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
# Creates a goal to send to the action server.
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint',
'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
goal.trajectory.points = []
point0 = JointTrajectoryPoint()
point0.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point0.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# To be reached 1 second after starting along the trajectory
point0.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point0)
point1 = JointTrajectoryPoint()
point1.positions = [-0.3, 0.2, -0.1, -1.2, 1.5, 0.5, 0.0]
point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0]
point1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point1.time_from_start = rospy.Duration(2.0)
goal.trajectory.points.append(point1)
point2 = JointTrajectoryPoint()
point2.positions = [0.0, 0.1, 0.0, 0.0, 0.0, 0.1, 0.0]
point2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point2.time_from_start = rospy.Duration(3.0)
def trayectoria_2():
pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10)
rospy.init_node('trayectoria_2', anonymous=True)
trajectory = JointTrajectory()
trajectory.joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "gripper_revolute_joint"]
trajectory.header.stamp = rospy.Time.now()
trajectory.header.frame_id = "base_footprint";
#Posicion default
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp.velocities = [0.1, 0.1, 0.1, 0.1, 0.0, 0.0]
jtp.time_from_start = rospy.Duration(3)
trajectory.points.append(jtp)
#Aproximacion a la pieza
jtp_2 = JointTrajectoryPoint()
jtp_2.positions = [0.0, 0.8560, 0.5522, -1.3622, 0.0, 0.0]
jtp_2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_2.time_from_start = rospy.Duration(6)
trajectory.points.append(jtp_2)
#Cierra la pinza
jtp_3 = JointTrajectoryPoint()
jtp_3.positions = [0.0, 0.8560, 0.5522, -1.3622, 0.0, 1.5]
jtp_3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_3.time_from_start = rospy.Duration(9)
trajectory.points.append(jtp_3)
#Sube
jtp_4 = JointTrajectoryPoint()
jtp_4.positions = [0.0, -0.2362, -0.5844, 0.4488, 0.0, 1.5]
jtp_4.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_4.time_from_start = rospy.Duration(12)
trajectory.points.append(jtp_4)
#Giro del brazo
jtp_5 = JointTrajectoryPoint()
jtp_5.positions = [1.6, -0.2362, -0.5844, 0.4488, 0.0, 1.5]
jtp_5.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_5.time_from_start = rospy.Duration(15)
trajectory.points.append(jtp_5)
#Aproximacion a destino
jtp_6 = JointTrajectoryPoint()
jtp_6.positions = [1.6, -0.1212, -0.8375, 0.9741, 0.0, 1.5]
jtp_6.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_6.time_from_start = rospy.Duration(18)
trajectory.points.append(jtp_6)
#Suelta la pieza
jtp_7 = JointTrajectoryPoint()
jtp_7.positions = [1.6, -0.1212, -0.8375, 0.9741, 0.0, 0.0]
jtp_7.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_7.time_from_start = rospy.Duration(21)
trajectory.points.append(jtp_7)
#Gira de vuelta
jtp_8 = JointTrajectoryPoint()
jtp_8.positions = [0.0, -0.1212, -0.8375, 0.9741, 0.0, 0.0]
jtp_8.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_8.time_from_start = rospy.Duration(24)
trajectory.points.append(jtp_8)
#Posicion default
jtp_9 = JointTrajectoryPoint()
jtp_9.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_9.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp_9.time_from_start = rospy.Duration(27)
trajectory.points.append(jtp_9)
pub.publish(trajectory)
def main(whole_body, gripper, wrist_wrench):
rospy.sleep(5)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
#1.READ THE ANGLE (topic angle_detector)
#angle_Msg = rospy.wait_for_message("angle_detector", Float64)
#angle = angle_Msg.data
#2.READ THE HANDLE POSITION when the door is closed - SERVICE!!!
#target_pose_Msg = rospy.wait_for_message("localize_handle", PoseStamped)
#recog_pos.pose.position.x=target_pose_Msg.pose.position.x
#recog_pos.pose.position.y=target_pose_Msg.pose.position.y
#recog_pos.pose.position.z=target_pose_Msg.pose.position.z
if (angle > -1 and angle < 4): #the door is closed
##3.GRAB THE DOOR HANDLE
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
#4.ROTATE HANDLE
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
wrist_roll = latest_positions["wrist_roll_joint"] - 0.55
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
whole_body.impedance_config = 'grasping'
#5.PUSH MOTION - he moves with the door - first option - streight movement
phi = 90 * (2 * math.pi / 360)
l = HANDLE_TO_DOOR_HINGE_POS
d = 2 * l * math.cos(phi)
#open_pose = geometry.pose(x = d*math.sin(phi), y=d*math.cos(phi), ek=math.pi/2 - angle_rad )
#whole_body.move_end_effector_pose(open_pose, _ROBOT_TF)
#angleeee=math.pi/2 - angle_rad
omni_base.go(d * math.sin(phi),
d * math.cos(phi),
math.pi / 2 - angle_rad,
300.0,
relative=True)
whole_body.move_to_neutral()
elif (angle >= 4 and angle < 60): #the door is half-open
#3.ROTATE PARALLEL TO THE DOOR
angle_rad = angle * (2 * math.pi / 360)
#omni_base.go_rel(0.0, 0.0, angle_rad , 300.0)
##4.GRAB THE DOOR HANDLE
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(0.0)
#change coordinates of the handle - now the door is open so the handle is moved - the data of the handle position are given for the closed door - I m supposing
#that the coordinates of the handle are wrt the base_footprint tf
phi = math.pi / 2 - angle_rad / 2
l = HANDLE_TO_DOOR_HINGE_POS
d1 = 2 * l * math.sin(angle_rad / 2)
recog_pos.pose.position.x = recog_pos.pose.position.x + d1 * math.sin(
phi)
recog_pos.pose.position.y = recog_pos.pose.position.y + d1 * math.cos(
phi)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y - 10,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
#5.PUSH MOTION - he moves with the door - first option - streight movement
phi = math.pi / 4 + angle_rad / 2
l = HANDLE_TO_DOOR_HINGE_POS
d = 2 * l * math.cos(phi)
omni_base.go(d * math.sin(phi),
d * math.cos(phi),
math.pi / 2 - angle_rad,
300.0,
relative=True)
queue_size=10)
rospy.sleep(0.5)
traj = JointTrajectory()
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now
p1 = JointTrajectoryPoint()
p1.positions = [1.5, -0.2, -1.57, 0, 0, 0]
p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p1.time_from_start = rospy.Duration(5.0)
traj.points.append(p1)
p2 = JointTrajectoryPoint()
p2.positions = [1.5, 0, -1.57, 0, 0, 0]
p2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p2.time_from_start = rospy.Duration(10.0)
traj.points.append(p2)
p3 = JointTrajectoryPoint()
p3.positions = [2.2, 0, -1.57, 0, 0, 0]
p3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p3.time_from_start = rospy.Duration(15.0)
traj.points.append(p3)
# wait for the action server to establish connection
cli.wait_for_server()
head_cli.wait_for_server()
traj_goal = FollowJointTrajectoryGoal()
traj = JointTrajectory()
traj.joint_names = [
"odom_x", "odom_y", "odom_t", "arm_lift_joint", "arm_flex_joint",
"arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
p.positions = [0.0, 0.0, 0.0, 0.69, -2.60, 0.0, -0.54, 0.0]
p.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
traj_goal.trajectory = traj
cli.send_goal(traj_goal)
cli.wait_for_result()
head_traj_goal = FollowJointTrajectoryGoal()
head_traj = JointTrajectory()
head_traj.joint_names = ["head_pan_joint", "head_tilt_joint"]
h_p = JointTrajectoryPoint()
import rospy
from trajectory_msgs.msg import JointTrajectoryPoint
''' Script for interacting with group_node from an interactive sessions
run as "python -i group_setpoint_control.py"
Modify the fields of 'msg' in the python console
to change the setpoint of the actuator group
'''
if __name__ == "__main__":
rospy.init_node("trajectory_gen")
t_pub = rospy.Publisher("/joint_target",
JointTrajectoryPoint,
queue_size=1)
msg = JointTrajectoryPoint()
msg.positions = [0, 0, 0]
msg.velocities = [0, 0, 0]
msg.accelerations = [0, 0, 0]
timer = rospy.Timer(rospy.Duration(0.1), lambda x: t_pub.publish(msg))
print("Modify 'msg' variable to control arm...")
def __init__(self):
self.rate = rospy.Rate(10)
self.sentencec = "待機中"
# topics
# self.sub_joint = rospy.Subscriber("/hsrb/joint_states", JointState ,self.now_pose,queue_size=10)
self.sub = rospy.Subscriber("/hsrb_07/joy",
Joy,
self.callback,
queue_size=10)
rospy.Subscriber("/hsrb/wrist_wrench/compensated",
WrenchStamped,
self.cb_wrench_compensated,
queue_size=10)
self.grip_act = rospy.Publisher("/hsrb/gripper_controller/grasp/goal",
GripperApplyEffortActionGoal,
queue_size=10)
self.pub_list = rospy.Publisher('/hsrb/gripper_controller/command',
JointTrajectory,
queue_size=10)
self.sub_obj = rospy.Subscriber("/hsr7/recog_obj/name",
String,
self.recog_obj,
queue_size=10)
# self.vision_obj = rospy.Subscriber("/ext/vision_module/object_recognition_info", ObjectInfo, self.recog_vision,queue_size=10)
self.pub_sig = rospy.Publisher(
'/hsrb/gripper_trajectory_controller/command',
JointTrajectory,
queue_size=10)
rospy.Subscriber("/AudioSentence",
AudioSentence,
self.set_audio,
queue_size=10)
# service
#
self.hand_traj = JointTrajectory()
self.hand_sig = JointTrajectory()
self.hand_traj.joint_names = ["hand_motor_joint"]
self.hand_sig.joint_names = [
"hand_l_proximal_joint", "hand_r_proximal_joint"
]
pp = JointTrajectoryPoint()
pp.positions = [
1.2,
]
pp.velocities = [0]
pp.effort = [0.1]
pp.time_from_start = rospy.Time(3)
self.hand_traj.points = [pp]
self.hand_sig.points = [pp]
self.hand_flug = True
self.srv_record_start = rospy.ServiceProxy("rosbag_record",
RosbagRecord)
self.srv_record_stop = rospy.ServiceProxy("rosbag_record_stop",
RosbagStop)
self.srv_marker = rospy.ServiceProxy("marker_data_saver", Empty)
self.rosbag_number = 0
self.record_flag = False
self.rosbag_time = rospy.Time(0)
self.record_number = 0
self.number = 0
date = time.ctime()
date = date.replace(" ", " ")
self.date = date.split(" ")
self.image_number = 0
self.image_time = 0
# TF
self.buf = tf2_ros.Buffer()
self.lis = tf2_ros.TransformListener(self.buf)
self._wrench = WrenchStamped()
self.obj_name = ""
self.obj_time = rospy.Time.now().to_sec()
self.vis_obj_name = ""
self.vis_obj_num = 0
self.vis_obj_time = rospy.Time.now().to_sec()
self.flag_record = True
def compose_trajectory(self, component_name, parameter_name):
# get joint_names from parameter server
param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
if not rospy.has_param(param_string):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
return (JointTrajectory(), 2)
joint_names = rospy.get_param(param_string)
# check joint_names parameter
if not type(joint_names) is list: # check list
rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
print "joint_names are:",joint_names
return (JointTrajectory(), 3)
else:
for i in joint_names:
#print i,"type1 = ", type(i)
if not type(i) is str: # check string
rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
print "joint_names are:",param
return (JointTrajectory(), 3)
else:
rospy.logdebug("accepted joint_names for component %s",component_name)
'''
# get joint values from parameter server
if type(parameter_name) is str:
full_parameter_name = self.ns_global_prefix + "/" + component_name + "/" + parameter_name # parameter_name: joint_state
if not rospy.has_param(full_parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",full_parameter_name)
return (JointTrajectory(), 2)
param = rospy.get_param(full_parameter_name)
else:
param = parameter_name
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
print "parameter is:",param
return (JointTrajectory(), 3)
'''
param = self.points
traj = []
for point in param:
#print point,"type1 = ", type(point)
if type(point) is str:
full_parameter_name = self.ns_global_prefix + "/" + component_name + "/" + point
if not rospy.has_param(full_parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",full_parameter_name)
return (JointTrajectory(), 2)
point = rospy.get_param(full_parameter_name)
point = point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
#print point
elif type(point) is list:
rospy.logdebug("point is a list")
else:
rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
print "parameter is:",param
return (JointTrajectory(), 3)
# here: point should be list of floats/ints
#print point
if not len(point) == len(joint_names): # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
print "parameter is:",param
return (JointTrajectory(), 3)
for value in point:
#print value,"type2 = ", type(value)
if not ((type(value) is float) or (type(value) is int)): # check type
#print type(value)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
print "parameter is:",param
return (JointTrajectory(), 3)
rospy.logdebug("accepted value %f for %s",value,component_name)
traj.append(point)
rospy.logdebug("accepted trajectory for %s",component_name)
# get current pos
timeout = 3.0
try:
start_pos = rospy.wait_for_message("/" + component_name + "/joint_states", JointState, timeout = timeout).position
except rospy.ROSException as e:
rospy.logwarn("no joint states received from %s within timeout of %ssec. using default point time of 8sec.", component_name, str(timeout))
start_pos = []
# convert to ROS trajectory message
traj_msg = JointTrajectory()
# if no timestamp is set in header, this means that the trajectory starts "now"
traj_msg.joint_names = joint_names
point_nr = 0
traj_time = 0
param_string = self.ns_global_prefix + "/" + component_name + "/default_vel"
if not rospy.has_param(param_string):
default_vel = 0.1 # rad/s
rospy.logwarn("parameter %s does not exist on ROS Parameter Server, using default of %f [rad/sec].",param_string,default_vel)
else:
default_vel = rospy.get_param(param_string)
param_string = self.ns_global_prefix + "/" + component_name + "/default_acc"
if not rospy.has_param(param_string):
default_acc = 1.0 # rad^2/s
rospy.logwarn("parameter %s does not exist on ROS Parameter Server, using default of %f [rad^2/sec].",param_string,default_acc)
else:
default_acc = rospy.get_param(param_string)
for point in traj:
point_nr = point_nr + 1
point_msg = JointTrajectoryPoint()
point_msg.positions = point
# set zero velocities for last trajectory point only
#if point_nr == len(traj):
# point_msg.velocities = [0]*len(joint_names)
# set zero velocity and accelerations for all trajectory points
point_msg.velocities = [0]*len(joint_names)
point_msg.accelerations = [0]*len(joint_names)
# use hardcoded point_time if no start_pos available
if start_pos != []:
print "start pose is not defined"
point_time = self.calculate_point_time(component_name, start_pos, point, default_vel, default_acc)
else:
print "********Hello*******"
point_time = 8*point_nr
start_pos = point
point_msg.time_from_start=rospy.Duration(point_time + traj_time)
traj_time += point_time
traj_msg.points.append(point_msg)
return (traj_msg, 0)
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
start_theta = base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.0675
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
print("finishing rotating handle")
## Move by End-effector line
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
print("push the door")
## Move base with linear & Angular motion
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
## Move base with linear & Angular motion second
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.5
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
rospy.sleep(2.0)
tw_cmd1 = geometry_msgs.msg.Twist()
tw_cmd1.linear.x = 0.6
tw_cmd1.linear.y = 0.2
tw_cmd1.angular.z = 0.25
vel_pub.publish(tw_cmd1)
# rospy.sleep(4.0)
rospy.sleep(3.0)
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = 0.65
tw_cmd2.linear.y = 0.5
tw_cmd2.angular.z = 0.35
vel_pub.publish(tw_cmd2)
# rospy.sleep(4.0)
rospy.sleep(2.0)
## Open the gripper
gripper.command(1.0)
## Move back
base.go_rel(-1.3, 0.0, start_theta)
gripper.command(1.0)
whole_body.move_to_neutral()
res.success = True
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
res.success = False
return res
control_pan_pos_2 = makeSimpleProfile(control_pan_pos_2,
target_pan_pos_2,
(POS_STEP_SIZE / 2.0))
control_tilt_pos_2 = makeSimpleProfile(control_tilt_pos_2,
target_tilt_pos_2,
(POS_STEP_SIZE / 2.0))
head_msg.header.stamp = rospy.Time.now()
# jtp_msg.points.positions = [control_pan_pos,control_tilt_pos,control_yaw_pos]
jtp_msg.positions = [
control_pan_pos, control_tilt_pos, control_pan_pos_2,
control_tilt_pos_2
]
jtp_msg.velocities = [0.5, 0.5, 0.5, 0.5]
jtp_msg.time_from_start = rospy.Duration.from_sec(0.00000002)
head_msg.points.append(jtp_msg)
pub.publish(twist)
head_pub.publish(head_msg)
except:
print e
finally:
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
def __init__(self):
# First set up service
request_trajectory_service = rospy.ServiceProxy(
"generate_toppra_trajectory", GenerateTrajectory)
# This is an example for UAV and output trajectory is converted
# accordingly
trajectory_pub = rospy.Publisher('multi_dof_trajectory',
MultiDOFJointTrajectory,
queue_size=1)
time.sleep(0.5)
# Example of a simple square trajectory for quadcopter. All vectors
# must be of same length
x = [0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0]
y = [0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0]
z = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# Another example. Same square defined through more points. This will
# overwrite the first example
x = [0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0, 0.0, 0.0]
y = [0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0]
z = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0]
# Create a service request which will be filled with waypoints
request = GenerateTrajectoryRequest()
# Add waypoints in request
waypoint = JointTrajectoryPoint()
for i in range(0, len(x)):
# Positions are defined above
waypoint.positions = [x[i], y[i], z[i], yaw[i]]
# Also add constraints for velocity and acceleration. These
# constraints are added only on the first waypoint since the
# TOPP-RA reads them only from there.
if i == 0:
waypoint.velocities = [2, 2, 2, 1]
waypoint.accelerations = [1.25, 1.25, 1.25, 1]
# Append all waypoints in request
request.waypoints.points.append(copy.deepcopy(waypoint))
# Set up joint names. This step is not necessary
request.waypoints.joint_names = ["x", "y", "z", "yaw"]
# Set up sampling frequency of output trajectory.
request.sampling_frequency = 100.0
# Set up number of gridpoints. The more gridpoints there are,
# trajectory interpolation will be more accurate but slower.
# Defaults to 100
request.n_gridpoints = 500
# If you want to plot Maximum Velocity Curve and accelerations you can
# send True in this field. This is intended to be used only when you
# have to debug something since it will block the service until plot
# is closed.
request.plot = False
# Request the trajectory
response = request_trajectory_service(request)
# Response will have trajectory and bool variable success. If for some
# reason the trajectory was not able to be planned or the configuration
# was incomplete or wrong it will return False.
print("Converting trajectory to multi dof")
joint_trajectory = response.trajectory
multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory(
joint_trajectory)
trajectory_pub.publish(multi_dof_trajectory)
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = [0.0] * len(self._goal.trajectory.joint_names)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def add_point(self, positions, velocities, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = copy(velocities)
point.time_from_start = time
self._goal.trajectory.points.append(point)
omega = np.pi / time
d_x = np.zeros(3)
d_x[0] = -omega * r * np.sin(theta)
d_x[1] = 0
d_x[2] = omega * r * np.cos(theta)
# Computing devirative of velocity
dd_x = np.zeros(3)
dd_x[0] = -(omega**2) * r * np.cos(theta)
dd_x[1] = 0
dd_x[2] = -(omega**2) * r * np.sin(theta)
msg = JointTrajectoryPoint()
msg.positions = [x[0], x[1], x[2]]
msg.velocities = [d_x[0], d_x[1], d_x[2]]
msg.accelerations = [dd_x[0], dd_x[1], dd_x[2]]
pub.publish(msg)
rospy.sleep(Ts)
# Last trajectory point means the system is stable, i.e. no velocity nor acceleration
x = np.zeros(3)
x[0] = x_e[0] - 2 * r
x[1] = 0
x[2] = x_e[2]
d_x = np.zeros(3)
dd_x = np.zeros(3)
# Hand gesture parameters do not need an array and are defined as: # # [thumb, pointer, middle, ring, pinky] # # # # Arm gesture parameters only include shoulder_medial_joint and elbow_joint. # # All others in the array should be set as 0. # # =========================================================================== # pointer = [1, 0, 1, 1, 1] peace = [1, 0, 0, 1, 1] thumbs_up = [0, 1, 1, 1, 1] fist = [1, 1, 1, 1, 1] open_hand = [0, 0, 0, 0, 0] pointer_gest = JointTrajectory() pointer_point = JointTrajectoryPoint() pointer_point.positions = pointer pointer_point.velocities = [0] * 5 pointer_gest.points = [pointer_point] hand_gesture1 = JointTrajectory() hand_gesture1.points = [JointTrajectoryPoint(positions=pointer)] hand_gesture2 = JointTrajectory() hand_gesture2.points = [JointTrajectoryPoint(positions=peace)] hand_gesture3 = JointTrajectory() hand_gesture3.points = [JointTrajectoryPoint(positions=thumbs_up)] hand_gesture4 = JointTrajectory() hand_gesture4.points = [JointTrajectoryPoint(positions=fist)] hand_gesture5 = JointTrajectory() hand_gesture5.points = [JointTrajectoryPoint(positions=open_hand)] gesture_state = String() hand_gesture = JointTrajectory()
def createTrajectoryPoint(time, positions):
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration(time)
point.positions = positions
point.velocities = ZERO_VECTOR
return point
imutime = 0 #for record
initialTheta = 0 #laser_scan initial theta radient
initialThetalist = []
statusJP = JointTrajectoryPoint()
statusJP_time = 0
pose_time = 0
ready = 0
enginevalue = 1500
steeringvalue = 1500
started = True
MAX_engine = 1620
#initialize goal
goal.positions = [1, 1, 0]
goal.velocities = [0, 0, 0]
goal.accelerations = [0, 0, 0]
goal.effort = [1]
#initialize statusJP
statusJP.positions = [0, 0, 0]
statusJP.velocities = [0, 0, 0]
statusJP.accelerations = [0, 0, 0]
statusJP.effort = [1]
def mpcControl(initial_state, cx, cy, cyaw, cv, ca, cj):
"""
:param x1:
:param y1:
:param yaw1:
def main(whole_body, base, gripper, wrist_wrench):
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
# publisher for delvering command for base move
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
# base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
#test with service - get the handle position from handle
grasp_point_client()
global recog_pos
global Is_found
print recog_pos.pose.position
# target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
# recog_pos.pose.position.x=target_pose_Msg.pose.position.x
# recog_pos.pose.position.y=target_pose_Msg.pose.position.y
# recog_pos.pose.position.z=target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_,
y=recog_pos.pose.position.y - 0.012,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
# goal_pose =PoseStamped()
# goal_pose.pose.position.x=recog_pos.pose.position.x+0.4
# goal_pose.pose.position.y=recog_pos.pose.position.y+0.2
# goal_pose.pose.position.z=recog_pos.pose.position.z
# goal_pose.pose.orientation.x=recog_pos.pose.orientation.x
# goal_pose.pose.orientation.y=recog_pos.pose.orientation.y
# goal_pose.pose.orientation.z=recog_pos.pose.orientation.z
# goal_pose.pose.orientation.w=recog_pos.pose.orientation.w
# yaw=math.pi/6
# quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
# #type(pose) = geometry_msgs.msg.Pose
# nav_goal = move_base_msgs.msg.MoveBaseActionGoal()
# nav_goal.header.stamp=rospy.Time(0)
# nav_goal.goal.target_pose.header.frame_id = "map"
# nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3
# nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2
# nav_goal.goal.target_pose.pose.position.z=0.0
# nav_goal.goal.target_pose.pose.orientation.x=quaternion[0]
# nav_goal.goal.target_pose.pose.orientation.y=quaternion[1]
# nav_goal.goal.target_pose.pose.orientation.z=quaternion[2]
# nav_goal.goal.target_pose.pose.orientation.w=quaternion[3]
# base_pub.publish(nav_goal)
# # whole_body.end_effector_frame = u'odom'
# # whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# # publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
# # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5)
# # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5)
# whole_body.impedance_config= None
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.3
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
# rospy.sleep(4.0)
rospy.sleep(4.0)
gripper.command(1.0)
# whole_body.move_end_effector_by_line((0, 0, -1), 0.25)
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -1.1
tw_cmd2.angular.z = -0.4
vel_pub.publish(tw_cmd2)
rospy.sleep(4.0)
whole_body.move_to_neutral()
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -0.6
tw_cmd2.angular.z = -0.3
vel_pub.publish(tw_cmd2)
# DEFINITIONS # # --------------------------------------------------------------------------- # # Arm gesture parameters only include shoulder_medial_joint and elbow_joint. # # All others in the array should be set empty. # # 2 is the max value for elbow_joint, while -2 is the min # # 3.2 indicates a 180 degree rotation for shoulder_medial_joint # # =========================================================================== # arm_names = ['shoulder_medial_joint', 'elbow_joint'] # Blank gesture that is set depending on the desired position in the main loop arm_gesture = JointTrajectory() arm_gesture.joint_names = arm_names arm_gesture_points = JointTrajectoryPoint() arm_gesture_points.positions = [] arm_gesture_points.velocities = [] arm_gesture_points.accelerations = [] arm_gesture_points.effort = [] arm_gesture_points.time_from_start = rospy.Duration(3) arm_gesture.points = [arm_gesture_points] position = String() # point locations # test = [-1.37, -1.54] test = [-1.6, -1.3] default = [0, 0] max_left = [0, 2] max_right = [3.2, 2] middle = [1.6, 1]
def get_trajectory_point(positions, time_from_start):
point = JointTrajectoryPoint()
point.positions = positions
point.velocities = [.0] * len(positions)
point.time_from_start = time_from_start
return point
