def motor_pub():
p1 = rospy.Publisher('/gBot/left_wheel/cmd', JointState)
p2 = rospy.Publisher('/gBot/right_wheel/cmd', JointState)
rospy.init_node('motorPub', anonymous=True)
rospy.on_shutdown(motor_stop)
#Message for left_wheel
msg1 = JointState()
msg1.name = ["left_wheel"]
msg1.position = [0.0]
msg1.velocity = [-100]
#Message for right_wheel
msg2 = JointState()
msg2.name = ["right_wheel"]
msg2.position = [0.0]
msg2.velocity = [-100]
while not rospy.is_shutdown():
msg1.header.stamp = rospy.Time.now()
p1.publish(msg1)
msg2.header.stamp = rospy.Time.now()
p2.publish(msg2)
rospy.loginfo('Left Motor velocity')
rospy.loginfo(msg1.velocity)
rospy.loginfo('Right Motor velocity')
rospy.loginfo(msg2.velocity)
rospy.sleep(0.1)
def execute(self, userdata):
# rospy.loginfo('Executing state %s', self.__class__.__name__)
# self.active = True
# self.first_call = True
# # wait for some time to read from the topic
# # TODO: replace with a service call
# rospy.sleep(3)
# userdata.obj_list = self.obj_list
# self.active = False
# return 'succeeded'
rospy.loginfo("Executing state %s", self.__class__.__name__)
self.obj_list = []
i = 1
for view in userdata.view_list:
rospy.loginfo("%i view: set PTU to %s", i, view)
joint_state = JointState()
joint_state.header.frame_id = "tessdaf"
joint_state.name = ["pan", "tilt"]
joint_state.position = [float(view[0]), float(view[1])]
joint_state.velocity = [float(1.0), float(1.0)]
joint_state.effort = [float(1.0), float(1.0)]
self.ptu_cmd.publish(joint_state)
# wait until PTU has finished or point cloud get screwed up
rospy.sleep(2)
rospy.loginfo("%i view: receive from semantic camera", i)
self.active = True
self.first_call = True
# wait for some time to read once from the topic and store it onto self.pointcloud
# TODO: replace with a service call
rospy.sleep(2)
self.active = False
i = i + 1
userdata.obj_list = self.obj_list
# back to original position
joint_state = JointState()
joint_state.header.frame_id = "tessdaf"
joint_state.name = ["pan", "tilt"]
joint_state.position = [float(0.0), float(0.0)]
joint_state.velocity = [float(1.0), float(1.0)]
joint_state.effort = [float(1.0), float(1.0)]
self.ptu_cmd.publish(joint_state)
return "succeeded"
def execute(self, goal):
pickle.dump(goal, open("trajecoryGoalMsgVrep.data", "wb"))
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
num_points = len(trajectory_points)
rospy.loginfo("Received %i trajectory points" % num_points)
pprint(goal.trajectory)
end_time = trajectory_points[-1].time_from_start.to_sec()
control_rate = rospy.Rate(self._control_rate)
i=num_points
rospy.loginfo("the num of point is %i" % i )
#self.jointTrajectoryPublisher.publish(goal.trajectory)
for trajectory_point in trajectory_points:
#Publish for vrepp
jsVrep = JointState()
jsVrep.name = copy.deepcopy(joint_names)
jsVrep.position = trajectory_point.positions
for j in range(len(joint_names)):
jsVrep.name[j] = self.vrep_joint_names[joint_names[j]]
vrep_joint_index = int(jsVrep.name[j][-1]) - 1
print vrep_joint_index
if vrep_joint_index == 4:
self.jointStatePublisherForVrep[vrep_joint_index].publish((-1*jsVrep.position[j]) + self.vrep_position_offset[vrep_joint_index])
else:
self.jointStatePublisherForVrep[vrep_joint_index].publish(jsVrep.position[j] + self.vrep_position_offset[vrep_joint_index])
#Publisher for real robot
js = JointState()
#rospy.loginfo("the length of js.name is %i" %len(js.name))
js.name=copy.deepcopy(joint_names)
for j in range(len(joint_names)):
#rospy.loginfo("j is %i" % j)
js.name[j] = js.name[j]+'_'+str(i)
i=i-1
js.position = trajectory_point.positions
self.jointStatePublisher.publish(js)
control_rate.sleep()
start_time = rospy.get_time()
now_from_start = rospy.get_time() - start_time
last_idx = -1
self._result.error_code = self._result.SUCCESSFUL
self.server.set_succeeded(self._result)
rospy.sleep(5)
self.execution.publish("OK")
def gripper_cb(self, data):
js = JointState()
js.header = data.header
js.name = ["Gripper"]
js.position = [data.current_pos]
js.velocity = [data.velocity]
self.gripper_pub.publish(js)
def handstate_callback(handstate_msg):
assert len(handstate_msg.positions) == 4
assert len(handstate_msg.inner_links) == 3
assert len(handstate_msg.outer_links) == 3
jointstate_msg = JointState()
jointstate_msg.header.stamp = handstate_msg.header.stamp
jointstate_msg.name = [
joint_prefix + '00',
# Joint j01 is a mimic joint that is published by robot_state_publisher.
joint_prefix + '01',
joint_prefix + '11',
joint_prefix + '21',
# TODO: These are not currently being published. See below.
#joint_prefix + '02',
#joint_prefix + '12',
#joint_prefix + '22',
]
jointstate_msg.position += [ handstate_msg.positions[3] ]
jointstate_msg.position += handstate_msg.inner_links
# TODO: These positions look very wrong, so we'll let robot_state_publisher
# use the mimic ratios in HERB's URDF to compute the distal joint angles.
#jointstate_msg.position += handstate_msg.outer_links
jointstate_pub.publish(jointstate_msg)
def joint_trajectory_to_joint_state(self, joint_trajectory_msg, index):
joint_states_msg = JointState()
joint_states_msg.name = joint_trajectory_msg.joint_names
joint_states_msg.position = joint_trajectory_msg.points[index].positions
joint_states_msg.velocity = joint_trajectory_msg.points[index].velocities
joint_states_msg.effort = joint_trajectory_msg.points[index].effort
return joint_states_msg
def execute(self, goal):
#pickle.dump(goal, open("trajecoryGoalMsg3.data", "wb"))
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
num_points = len(trajectory_points)
rospy.loginfo("Received %i trajectory points" % num_points)
pprint(goal.trajectory)
end_time = trajectory_points[-1].time_from_start.to_sec()
control_rate = rospy.Rate(self._control_rate)
i=num_points
rospy.loginfo("the num of point is %i" % i )
self.jointTrajectoryPublisher.publish(goal.trajectory)
for trajectory_point in trajectory_points:
js = JointState()
#rospy.loginfo("the length of js.name is %i" %len(js.name))
js.name=copy.deepcopy(joint_names)
for j in range(len(joint_names)):
#rospy.loginfo("j is %i" % j)
js.name[j] = js.name[j]+'_'+str(i)
i=i-1
#js.position = trajectory_point.positions
#js.velocity=trajectory_point.velocities
#self.jointStatePublisher.publish(js)
control_rate.sleep()
start_time = rospy.get_time()
now_from_start = rospy.get_time() - start_time
last_idx = -1
self._result.error_code = self._result.SUCCESSFUL
self.server.set_succeeded(self._result)
def rc_talker():
'''init pyxhook'''
#Create hookmanager
hookman = pyxhook.HookManager()
#Define our callback to fire when a key is pressed down
hookman.KeyDown = key_down_event
hookman.KeyUp = key_up_event
#Hook the keyboard
hookman.HookKeyboard()
#Start our listener
hookman.start()
pub = rospy.Publisher('rc_sender', String, queue_size=10)
# 2016-10-26 test urdf joint control
# controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch
#roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True
pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10)
rospy.init_node('rc_talker')
rate = rospy.Rate(50)
while not rospy.is_shutdown():
#hello_str = "hello world %s" % rospy.get_time()
pub.publish(''.join(rc_keys))
# 2016-10-26 test urdf joint control
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ['base_to_left_arm', 'base_to_right_arm']
msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])]
pub_2.publish(msg)
#print rc_keys
rate.sleep()
if running == False:
#Close the listener when we are done
hookman.cancel()
break
def eyelidServosPut(self,params):
msg = JointState()
msg.name = list()
msg.position = list()
msg.velocity = list()
msg.effort = list()
if params.has_key('leftEyelid_position'):
msg.name.append('left_eyelid_joint')
msg.position.append(math.radians(float(params['leftEyelid_position'])-float(90)))
if params.has_key('leftEyelid_speed'):
#Anadir velocuidad
msg.velocity.append(params['leftEyelid_speed'])
else:
msg.velocity.append(3.0)
if params.has_key('rightEyelid_position'):
msg.name.append('right_eyelid_joint')
msg.position.append(math.radians(float(params['rightEyelid_position'])-float(90)))
if params.has_key('rightEyelid_speed'):
#Anadir velocuidad
msg.velocity.append(params['rightEyelid_speed'])
else:
msg.velocity.append(3.0)
msg.header.stamp = rospy.Time.now()
self.cmd_joints_pub.publish(msg)
def sim_robot():
rospy.init_node("simulated_robot")
pub = rospy.Publisher("joint_states", JointState, queue_size=5)
rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))
msg = JointState()
msg.header = Header(stamp=rospy.Time.now())
msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"]
msg.position = [0] * 6
msg.velocity = [0, 0, 0, 0, 0, 0]
msg.effort = [0, 0, 0, 0, 0, 0]
def joint_cb(msg_in, id):
msg.header = Header(stamp=rospy.Time.now())
msg.position[id] = msg_in.data
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
def generateJointStatesFromRegisterStateAndPublish():
rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, updateLocalJointState)
global currentJointState
currentJointState = JointState()
currentJointState.header.frame_id = "robotiq_s_model from real-time state data"
currentJointState.name = ["finger_1_joint_1",
"finger_1_joint_2",
"finger_1_joint_3",
"finger_2_joint_1",
"finger_2_joint_2",
"finger_2_joint_3",
"finger_middle_joint_1",
"finger_middle_joint_2",
"finger_middle_joint_3",
"palm_finger_1_joint",
"palm_finger_2_joint"]
currentJointState.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
currentJointState.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
currentJointState.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pub = rospy.Publisher('joint_states', JointState, queue_size=1) #no buffering?
rospy.init_node('robotiq_s_model_joint_state_publisher', anonymous=False)
rate = rospy.Rate(100) # publishes at 100Hz
while not rospy.is_shutdown():
currentJointState.header.stamp = rospy.Time.now()
pub.publish(currentJointState)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def joint_state_publisher(self):
try:
joint_state_msg = JointState()
joint_fb_msg = FollowJointTrajectoryFeedback()
time = rospy.Time.now()
with self.lock:
#Joint states
joint_state_msg.header.stamp = time
joint_state_msg.name = self.joint_names
joint_state_msg.position = self.motion_ctrl.get_joint_positions()
#self.joint_state_pub.publish(joint_state_msg)
#Joint feedback
joint_fb_msg.header.stamp = time
joint_fb_msg.joint_names = self.joint_names
joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
#self.joint_feedback_pub.publish(joint_fb_msg)
except Exception as e:
rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def js_cb(self, a):
rospy.loginfo('Received array')
js = JointState()
js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
jList = a.data
jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
i = 0
for pos in jMatrix:
rospy.loginfo(pos)
js.position = pos
gsvr = GetStateValidityRequest()
rs = RobotState()
rs.joint_state = js
gsvr.robot_state = rs
gsvr.group_name = "both_arms"
resp = self.coll_client(gsvr)
if (resp.valid == False):
rospy.loginfo('false')
rospy.loginfo(i)
self.js_pub.publish(0)
return
i = i + 1
self.js_pub.publish(1)
rospy.loginfo('true')
return
def set_stiffness(self):
stiffness_message = JointState()
stiffness_message.name = ["HeadYaw", "HeadPitch"]
stiffness_message.position = [0,0]
stiffness_message.velocity = [0,0]
stiffness_message.effort = [1,1]
self.stiffness_publisher.publish(stiffness_message)
def publish_position():
'''joint position publisher'''
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = JOINT_NAME_ARRAY
msg.position = joints_pos
pub_js.publish(msg)
def resethead(): # rotate the head(not PTU)
pub = rospy.Publisher('/head/commanded_state', JointState)
head_command = JointState()
head_command.name=["HeadPan"]
head_command.position=[0.0] #forwards
pub.publish(head_command)
eyeplay(0)# reset eyes
def __on_packet(self, buf):
global last_joint_states, last_joint_states_lock
now = rospy.get_rostime()
#stateRT = RobotStateRT.unpack(buf)
stateRT = RobotStateRT_V32.unpack(buf)
self.last_stateRT = stateRT
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0]*6
pub_joint_states.publish(msg)
with last_joint_states_lock:
last_joint_states = msg
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = now
wrench_msg.wrench.force.x = stateRT.tcp_force[0]
wrench_msg.wrench.force.y = stateRT.tcp_force[1]
wrench_msg.wrench.force.z = stateRT.tcp_force[2]
wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
pub_wrench.publish(wrench_msg)
def _move_to(self, point, dur):
rospy.sleep(dur)
msg = JointState()
with self.lock:
if not self.sig_stop:
self.joint_positions = point.positions[:]
self.joint_velocities = point.velocities[:]
msg.name = ['joint_1',
'joint_2',
'joint_3',
'joint_4',
'joint_5',
'joint_6',
'joint_7']
#msg.name = self.joint_names
msg.position = self.joint_positions
msg.velocity = self.joint_velocities #[0.2]*7
msg.header.stamp = rospy.Time.now()
self.joint_command_pub.publish(msg)
#rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur))
else:
rospy.loginfo('Stoping motion immediately, clearing stop signal')
self.sig_stop = False
def default(self, ci="unused"):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
js.name = [
"kuka_arm_0_joint",
"kuka_arm_1_joint",
"kuka_arm_2_joint",
"kuka_arm_3_joint",
"kuka_arm_4_joint",
"kuka_arm_5_joint",
"kuka_arm_6_joint",
"head_pan_joint",
"head_tilt_joint",
]
js.position = [
self.data["seg0"],
self.data["seg1"],
self.data["seg2"],
self.data["seg3"],
self.data["seg4"],
self.data["seg5"],
self.data["seg6"],
self.data["pan"],
self.data["tilt"],
]
# js.velocity = [1, 1, 1, 1, 1, 1, 1]
# js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def update(self):
if not self.driver_status == 'DISCONNECTED':
# Get Joint Positions
self.current_joint_positions = self.rob.getj()
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "robot_secondary_interface_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0]*6
msg.effort = [0]*6
self.joint_state_publisher.publish(msg)
# Get TCP Position
tcp_angle_axis = self.rob.getl()
# Create Frame from XYZ and Angle Axis
T = PyKDL.Frame()
axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
# Get norm and normalized axis
angle = axis.Normalize()
# Make frame
T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
T.M = PyKDL.Rotation.Rot(axis,angle)
# Create Pose
self.current_tcp_pose = tf_c.toMsg(T)
self.current_tcp_frame = T
self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list):
if len(jointsToSweep) != 0:
for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping
currPositions[joint_list.index(jointsToSweep[0])]
currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])])
if (len(jointsToSweep) > 1):
self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list)
# Combine message and publish
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.joint_list
# print self.free_joints
msg.position = currPositions
msg.velocity = [0.0]*len(maxs)
msg.effort = [0.0]*len(maxs)
time.sleep(0.0001)
self.pub.publish(msg)
# Get End Effector Positions
rate = rospy.Rate(20.0)
try:
(trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0))
if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]):
self.csvWriter.writerow(trans)
self.currX = trans[0]
self.currY = trans[1]
self.currZ = trans[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "ERROR: looking up tf"
time.sleep(0.5) # Sleep for a bit to let system catch up
continue
def generate_views(self):
rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
try:
resp = self.nav_goals(10, self.inflation_radius, self.roi)
if not resp.goals.poses:
self.views = []
return
for j in range(len(resp.goals.poses)):
for i in range(0,self.views_at_pose):
pose = resp.goals.poses[j]
pan = random.uniform(self.min_pan, self.max_pan)
tilt = random.uniform(self.min_tilt, self.max_tilt)
jointstate = JointState()
jointstate.name = ['pan', 'tilt']
jointstate.position = [pan, tilt]
jointstate.velocity = [self.velocity, self.velocity]
jointstate.effort = [float(1.0), float(1.0)]
resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
p = Pose()
view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
self.views.append(view)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def publishJointState(self):
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.jointNames
msg.position = self.getJointAngles()
msg.effort = self.getLoads()
self.jointStatePub.publish(msg)
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
def head_wander():
rospy.init_node('head_wander')
joints_pub = rospy.Publisher('/cmd_joints', JointState)
say = rospy.ServiceProxy('/qbo_talk/festival_say', Text2Speach)
seq = 0
while not rospy.is_shutdown():
seq += 1
msg = JointState()
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_footprint'
msg.name = ['head_pan_joint','head_tilt_joint']
msg.position = [random.uniform(-0.9,0.9), random.uniform(-0.8,0.3)]
joints_pub.publish(msg)
if random.random() < 0.3:
try:
say(random.choice(UTTERANCES))
except rospy.ServiceException:
# Ignore errors
pass
rospy.sleep(2.0)
def publishJointStates(self):
jState = JointState()
jState.header.stamp = rospy.Time.now()
jState.name = self.names
jState.position = self.positions
jState.velocity = self.velocities
jState.effort = self.efforts
self.pub.publish(jState)
def __init__(self):
global jspr2msg
jspr2msg = JointState()
jspr2msg.name = [ "left_shoulder_tilt_joint", "left_lin_act_cyl_joint", "left_upper_arm_hinge_joint","left_linear_actuator_joint", "right_shoulder_tilt_joint", "right_lin_act_cyl_joint", "right_upper_arm_hinge_joint","right_linear_actuator_joint", "torso_lift_joint", "wheel_linear_actuator_joint", "fl_anchor_rod_joint", "fl_push_rod_joint", "fl_caster_rotation_joint", "fr_anchor_rod_joint", "fr_push_rod_joint", "fr_caster_rotation_joint", "bl_anchor_rod_joint", "bl_push_rod_joint", "bl_caster_rotation_joint", "br_anchor_rod_joint", "br_push_rod_joint", "br_caster_rotation_joint"]
jspr2msg.position = [0.0 for name in jspr2msg.name]
jspr2msg.velocity = [0.0 for name in jspr2msg.name]
rospy.loginfo("done init")
def js_pub_cb(self, params):
js = JointState()
js.header.stamp = rospy.Time.now()
js.name = ['pan_joint', 'tilt_joint']
if self.joint_states[0] is None:
return
js.position = self.joint_states
self.js_pub.publish(js)
def get_new_JointState(pan, tilt): j = JointState() j.header.stamp = rospy.Time.now() j.name = ["panservo", "tiltservo"] j.position = [pan, tilt] j.velocity = [] j.effort = [] return j
def calibration(portName1, simulated):
print "INITIALIZING TORSO CALIBRATION..."
rospy.init_node("torso")
jointStates = JointState()
jointStates.name = ["spine_connect","waist_connect","shoulders_connect", "shoulders_left_connect", "shoulders_right_connect"]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose",Float32MultiArray, queue_size = 1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0,0,0]
rate = rospy.Rate(30)
if not simulated:
print "Torso.-> Trying to open serial port on \"" + portName1 + "\""
Roboclaw1.Open(portName1, 38400) #ttyACM0 --- M1: front --- M2: rear
address = 0x80
print "Torso.-> Serial port openned on \"" + portName1 + "\" at 38400 bps (Y)"
print "Torso.-> Clearing previous encoders readings"
a, bumper , b = Roboclaw1.ReadEncM1(address)
print "Torso.-> bumper ", bumper
Roboclaw1.BackwardM2(address, 127) #Abajo-
while bumper == 0 or bumper==1:
a, bumper , b = Roboclaw1.ReadEncM1(address)
print "Torso.->bumper ", bumper
print "Torso.->bumper ", bumper
Roboclaw1.BackwardM2(address, 0)
torsoPos = 0
Roboclaw1.SetEncM2(address, torsoPos)
Roboclaw1.WriteNVM(address)
Roboclaw1.ForwardM2(address, 127) #Arriba+
a, torsoPos , b = Roboclaw1.ReadEncM2(address)
a, bumper , b = Roboclaw1.ReadEncM1(address)
while torsoPos < 84866 or bumper==-1:
a, torsoPos , b = Roboclaw1.ReadEncM2(address)
a, bumper , b = Roboclaw1.ReadEncM1(address)
print "Torso.->bumper ", bumper
Roboclaw1.BackwardM2(address, 0)
a, torsoPos , b = Roboclaw1.ReadEncM2(address)
#print "Torso.-> MAX Torso Pos: ", torsoPos
Roboclaw1.SetEncM2(address, torsoPos)
Roboclaw1.WriteNVM(address)
Roboclaw1.Close()
else:
torsoPos = 84866;
msgCurrentPose.data[0] = (torsoPos*0.352)/169733 #-------------------pulsos a metros
msgCurrentPose.data[1] = 0.0
msgCurrentPose.data[2] = 0.0
pubTorsoPos.publish(msgCurrentPose)
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [(torsoPos*0.352)/169733, 0.0, 0.0, 0.0, 0.0]
pubJointStates.publish(jointStates)
import rospy
import random
import time
from sensor_msgs.msg import JointState
target = rospy.Publisher('joint_targets', JointState, queue_size=10)
rospy.init_node('head_random')
msg = JointState()
msg.name = ["neck_axis0", "neck_axis1", "neck_axis2"]
msg.velocity = [0, 0, 0]
msg.effort = [0, 0, 0]
while (not rospy.is_shutdown()):
n0 = 0 % random.uniform(-0.05, 0.05)
n1 = random.uniform(-0.2, 0)
n2 = random.uniform(-0.3, 0.3)
print("%f %f %f" % (n0, n1, n2))
msg.position = [n0, n2, n2]
target.publish(msg)
time.sleep(3)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt=0.
wheel_right_joint = 0.
tinc = degree
wheel_left_joint = 0.
angle = 0.
height = 0.
hinc = 0.005
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'base_footprint'
joint_state = JointState()
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = ['wheel_right_joint', 'wheel_left_joint']
joint_state.position = [wheel_right_joint, wheel_left_joint]
# update transform
# (moving in a circle with radius=2)
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle)*2
odom_trans.transform.translation.y = sin(angle)*2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# Create new robot state
tilt += tinc
if tilt < -0.5 or tilt > 0.0:
tinc *= -1
height += hinc
if height > 0.2 or height < 0.0:
hinc *= -1
wheel_right_joint += degree
angle += degree/4
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def callback(data, args):
global theta_now
global pub
tl = args[0]
a2 = args[1]
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
r2 = x**2 + y**2 + z**2
r = sqrt(r2)
if r < sqrt(a2**2 + tl**2 - 1.41 * a2 * tl) or r > a2 + tl:
rospy.logerr("Point exceeded workpace")
return
beta = abs(atan2(z, sqrt(x**2 + y**2)))
theta1_list = []
theta2_list = []
theta3_list = []
theta1_list += [atan2(y, x)]
theta1_list += [atan2(y, x) + 3.14]
theta2_list += [abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) - beta]
theta2_list += [-abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) - beta]
theta2_list += [
-abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) + beta - 3.14
]
theta2_list += [
abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) + beta - 3.14
]
theta3_list += [acos((r2 - a2**2 - tl**2) / (2 * a2 * tl))]
theta3_list += [-acos((r2 - a2**2 - tl**2) / (2 * a2 * tl))]
flag1 = True
flag2 = False
if abs(theta1_list[0] - theta_now[0]) > 6:
theta1 = theta1_list[0]
flag1 = True
elif abs(theta1_list[1] - theta_now[0]) > 6:
theta1 = theta1_list[1]
flag1 = False
elif abs(theta1_list[0] - theta_now[0]) < abs(theta1_list[1] -
theta_now[0]):
theta1 = theta1_list[0]
flag1 = True
else:
theta1 = theta1_list[1]
flag1 = False
if flag1:
if abs(theta2_list[0] - theta_now[1] + 0.01) < abs(theta2_list[1] -
theta_now[1]):
theta2 = theta2_list[0]
flag2 = True
else:
theta2 = theta2_list[1]
flag2 = False
else:
if abs(theta2_list[2] - theta_now[1]) < abs(theta2_list[3] -
theta_now[1]):
theta2 = theta2_list[2]
flag2 = False
else:
theta2 = theta2_list[3]
flag2 = True
if flag2:
theta3 = theta3_list[1]
else:
theta3 = theta3_list[0]
if abs(x) < 0.01 and abs(y) < 0.01:
theta1 = theta_now[0]
theta2 = theta_now[1]
theta3 = theta_now[2]
theta_now = [theta1, theta2, theta3]
msg = JointState()
msg.name = ['joint1', 'joint2', 'joint3']
msg.header.stamp = rospy.get_rostime()
msg.position = theta_now
pub.publish(msg)
def move_to_pose(self, target_pose, mode, move_duration=0):
"""
:param target_pose: [x,y,z,R,P,Y] or Pose msg
:type target_pose: list or Pose
:param mode: "trajectory" or "command"
:type mode: str
:param move_duration: time in seconds to complete move
:type move_duration: float
:return:
"""
xyz_bounds = [0.01, 0.01, 0.01]
wxyz_bounds = [0.05, 0.05, 0.05]
if not isinstance(target_pose, Pose):
# xyz
target_xyz = [0, 0, 0]
for i in range(3):
if target_pose[i] in ("", " "):
xyz_bounds[i] = 1e5
else:
target_xyz[i] = float(target_pose[i])
# RPY
target_rpy = [0, 0, 0] # Tait-Byran Extrinsic xyz Euler angles
for i in range(3):
if target_pose[3 + i] in ("", " "):
wxyz_bounds[i] = 1e5
else:
target_rpy[i] = float(target_pose[3 + i])
# wxyz
target_wxyz = transforms.quaternion_from_euler(
target_rpy[0], target_rpy[1], target_rpy[2],
axes='sxyz').tolist()
else:
target_xyz = [
target_pose.position.x, target_pose.position.y,
target_pose.position.z
]
target_wxyz = [
target_pose.orientation.w, target_pose.orientation.x,
target_pose.orientation.y, target_pose.orientation.z
]
print("Seed angles: {}".format(
[round(ang, 4) for ang in self.hebi_wrap.get_joint_positions()]))
print("Target End-Effector Pose...")
print(" xyz: {}".format(target_xyz))
print(" wxyz: {}".format(target_wxyz))
print("Bounds...")
print(" xyz: {}".format(xyz_bounds))
print(" wxyz: {}".format(wxyz_bounds))
# Get target joint angles
target_jt_angles = self.trac_ik.get_ik(
self.hebi_wrap.get_joint_positions(), target_xyz[0], target_xyz[1],
target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2],
target_wxyz[3], xyz_bounds[0], xyz_bounds[1], xyz_bounds[2],
wxyz_bounds[0], wxyz_bounds[1], wxyz_bounds[2])
if target_jt_angles is None:
print("NO JOINT SOLUTION FOUND FOR END-EFFECTOR POSE "
"\n(x: {}, y: {}, z: {}, W: {}, X: {}, Y: {}, Z: {})"
"\nPLEASE TRY AGAIN.".format(*target_xyz + target_wxyz))
return False
else:
if mode == "trajectory":
goal = TrajectoryGoal()
start_wp = WaypointMsg()
start_wp.names = self.hebi_mapping
start_wp.positions = self.hebi_wrap.get_joint_positions()
start_wp.velocities = [0] * len(self.hebi_mapping)
start_wp.accelerations = [0] * len(self.hebi_mapping)
goal.waypoints.append(start_wp)
end_wp = WaypointMsg()
end_wp.names = self.hebi_mapping
end_wp.positions = target_jt_angles
end_wp.velocities = [0] * len(self.hebi_mapping)
end_wp.accelerations = [0] * len(self.hebi_mapping)
goal.waypoints.append(end_wp)
goal.times.extend([0, move_duration])
self._hold_position = False
self.hebi_wrap.trajectory_action_client.send_goal_and_wait(
goal)
self._hold_joint_angles = self.hebi_wrap.get_joint_positions()
self._hold_position = True
else: # mode == "command"
cmd = JointState()
cmd.name = self.hebi_mapping
cmd.position = target_jt_angles
cmd.velocity = [0] * len(self.hebi_mapping)
cmd.effort = [0] * len(self.hebi_mapping)
self._hold_position = False
self.hebi_wrap.joint_state_publisher.publish(cmd)
self._hold_joint_angles = target_jt_angles
self._hold_position = True
return True
q = ikine_ur5(xd, q0)
# Resulting position (end effector with respect to the base link)
T = fkine_ur5(q)
print('Obtained value:\n', np.round(T,3))
# Red marker shows the achieved position
bmarker.xyz(T[0:3,3])
# Green marker shows the desired position
bmarker_des.xyz(xd)
# Objeto (mensaje) de tipo JointState
jstate = JointState()
# Asignar valores al mensaje
jstate.header.stamp = rospy.Time.now()
jstate.name = jnames
# Add the head joint value (with value 0) to the joints
jstate.position = q
# Loop rate (in Hz)
rate = rospy.Rate(100)
# Continuous execution loop
while not rospy.is_shutdown():
speed = 0.0001 + (0.001 - 0.0001) * ((joystick.axes[3] - (-1)) / (1 - (-1)))
# Desired position
xdnew = np.array([xd[0] + joystick.axes[0]*speed, xd[1] + joystick.axes[1]*speed, xd[2] + joystick.axes[5]*speed])
qnew = ikine_ur5(xdnew, q)
if qnew is not False:
xd = xdnew
def main():
print "INITIALIZING ARM NODE IN SIMULATION BY [EDD-II]"
###Connection with ROS
rospy.init_node("head_simul_node")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = [
"arm_flex_joint", "arm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
jointStates.position = [0, 0, 0, 0]
subPosition = rospy.Subscriber("/hardware/arm/goal_pose",
Float32MultiArray, callbackPos)
pubArmPose = rospy.Publisher("/hardware/arm/current_pose",
Float32MultiArray,
queue_size=1)
#For test, these lines can be unneeded
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubArmBattery = rospy.Publisher("/hardware/robot_state/right_arm_battery",
Float32,
queue_size=1)
loop = rospy.Rate(10)
global goalAngles
global goalGripper
global speeds
goalAngles = [0, 0, 0, 0]
angles = [0, 0, 0, 0]
speeds = [0.01, 0.01, 0.01, 0.01]
goalGripper = 0
gripper = 0
gripperSpeed = 0.1
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0, 0, 0, 0]
msgCurrentGripper = Float32()
msgCurrentGripper.data = 0
deltaAngles = [0, 0, 0, 0]
deltaGripper = 0
while not rospy.is_shutdown():
for i in range(len(deltaAngles)):
deltaAngles[i] = goalAngles[i] - angles[i]
if deltaAngles[i] > speeds[i]:
deltaAngles[i] = speeds[i]
if deltaAngles[i] < -speeds[i]:
deltaAngles[i] = -speeds[i]
angles[i] += deltaAngles[i]
jointStates.position[i] = angles[i]
msgCurrentPose.data[i] = angles[i]
deltaGripper = goalGripper - gripper
if deltaGripper > gripperSpeed:
deltaGripper = gripperSpeed
if deltaGripper < -gripperSpeed:
deltaGripper = -gripperSpeed
gripper += deltaGripper
jointStates.header.stamp = rospy.Time.now()
pubJointStates.publish(jointStates)
pubArmPose.publish(msgCurrentPose)
msgBattery = Float32()
msgBattery.data = 11.6
pubArmBattery.publish(msgBattery)
loop.sleep()
def solveIK(targetFrame):
ok, tree = parser.treeFromFile(rospkg.RosPack().get_path('rviz_animator') + "/models/robocam.xml")
chain = tree.getChain('base', 'link_camera')
plotter = Plotter()
# 1. Solve for J4, J5_initial, J6
# First, convert quaternion orientation to XZY order Euler angles
targetQuat = targetFrame.M.GetQuaternion() # Get quaternion from KDL frame (x, y, z, w)
pitch, yaw, roll = tf.transformations.euler_from_quaternion(targetQuat, axes='rxzy')
pitch_deg, yaw_deg, roll_deg = math.degrees(pitch), math.degrees(yaw), math.degrees(roll)
J4_origin_orientation = posemath.toMsg(chain.getSegment(2).getFrameToTip()).orientation
J4_offset = euler_from_quaternion([J4_origin_orientation.x, J4_origin_orientation.y, J4_origin_orientation.z, J4_origin_orientation.w])[0]
J4_raw, J5_initial, J6 = pitch, yaw, roll
J4 = J4_raw - J4_offset
# 1. Complete above
print("J4: {} J5_init: {} J6: {}".format(J4, J5_initial, J6))
chainAngles = PyKDL.JntArray(8)
chainAngles[5], chainAngles[6], chainAngles[7] = J4, J5_initial, J6
chainFK = PyKDL.ChainFkSolverPos_recursive(chain)
purpleFrame = PyKDL.Frame()
brownFrame = PyKDL.Frame()
purpleSuccess = chainFK.JntToCart(chainAngles, purpleFrame)
# print("Purple success {}".format(purpleSuccess))
print("Target Orientation:\n{}".format(targetFrame.M))
print("Result Orientation:\n{}".format(purpleFrame.M))
brownSuccess = chainFK.JntToCart(chainAngles, brownFrame, segmentNr=7)
# 2. Determine position of orange point
# First, construct KDL chain of the 3 links involved in J4-J6
cameraOffsetChain = tree.getChain('link_pitch', 'link_camera')
cameraJointAngles = PyKDL.JntArray(2)
cameraJointAngles[0], cameraJointAngles[1] = J5_initial, J6
cameraOffsetChainFK = PyKDL.ChainFkSolverPos_recursive(cameraOffsetChain)
cameraFrame = PyKDL.Frame()
success = cameraOffsetChainFK.JntToCart(cameraJointAngles, cameraFrame)
# print("FK Status: {}".format(success))
# print("Camera Frame: {}".format(cameraFrame))
# print("End Effector Joint Angles: {}".format([J4, J5_initial, J6]))
orangePoint = targetFrame.p - (purpleFrame.p - brownFrame.p)
plotter.addVector(targetFrame.p, "pink")
plotter.addVector(orangePoint, "orange")
plotter.addVector(purpleFrame.p, "purple")
plotter.addVector(brownFrame.p, "brown")
# print("Target Frame Position: {}".format(targetFrame.p))
# print("Camera Frame Position: {}".format(cameraFrame.p))
# print("Offset: {}".format(targetFrame.p - cameraFrame.p))
# 2. Complete:
# 3. Convert orange point to cylindrical coordinates
orange_X, orange_Y, orange_Z = orangePoint
orange_R = math.sqrt(orange_X ** 2 + orange_Y ** 2)
orange_Theta = math.atan2(orange_Y, orange_X) # Theta measured from global positive X axis
# 3. Complete: (above)
print("Orange R: {} Theta: {}".format(orange_R, math.degrees(orange_Theta)))
# 4. Solve for J2 and J3 in the idealized R-Z plane
targetVectorOrig = PyKDL.Vector(0, orange_R, orange_Z)
plotter.addVector(targetVectorOrig, "targetRZOrig")
# First, remove the fixed offsets from the wrist, elbow, and shoulder pieces
wristEndFrame = PyKDL.Frame()
wristStartFrame = PyKDL.Frame()
elbowEndFrame = PyKDL.Frame()
elbowStartFrame = PyKDL.Frame()
shoulderEndFrame = PyKDL.Frame()
shoulderStartFrame = PyKDL.Frame()
chainFK.JntToCart(chainAngles, wristEndFrame, segmentNr=7)
chainFK.JntToCart(chainAngles, wristStartFrame, segmentNr=5)
chainFK.JntToCart(chainAngles, elbowEndFrame, segmentNr=4)
chainFK.JntToCart(chainAngles, elbowStartFrame, segmentNr=3)
chainFK.JntToCart(chainAngles, shoulderEndFrame, segmentNr=2)
chainFK.JntToCart(chainAngles, shoulderStartFrame, segmentNr=0)
plotter.addVector(wristEndFrame.p, "wristEndFrame")
plotter.addVector(wristStartFrame.p, "wristStartFrame")
plotter.addVector(elbowEndFrame.p, "elbowEndFrame")
plotter.addVector(elbowStartFrame.p, "elbowStartFrame")
plotter.addVector(shoulderEndFrame.p, "shoulderEndFrame")
plotter.addVector(shoulderStartFrame.p, "shoulderStartFrame")
wristOffset = wristEndFrame.p - wristStartFrame.p
elbowOffset = elbowEndFrame.p - elbowStartFrame.p
shoulderOffset = shoulderEndFrame.p - shoulderStartFrame.p
targetVector = targetVectorOrig - wristOffset - elbowOffset - shoulderOffset
plotter.addVector(targetVector, "targetRZ")
# The following steps use the same labels as the classic 2D planar IK derivation
a1, a2 = (shoulderEndFrame.p - elbowStartFrame.p).Norm(), (elbowEndFrame.p - wristStartFrame.p).Norm()
_, ik_x, ik_y = targetVector
q2_a = math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2))
q1_a = math.atan2(ik_y, ik_x) - math.atan2(a2 * math.sin(-q2_a), a1 + a2 * math.cos(-q2_a))
q2_b = -1 * math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2))
q1_b = math.atan2(ik_y, ik_x) + math.atan2(a2 * math.sin(q2_b), a1 + a2 * math.cos(q2_b))
# Choose 'better' set of q1_ab, q2_ab
q1, q2 = q1_a, q2_a # TODO(JS): Is this always the better one?
J2_initial = q1
J2_offset = math.radians(90) # J2's zero position is vertical, not horizontal
J2 = J2_initial - J2_offset
# Since we have a parallel link, the angle for J3 is not simply q2. Instead, use transversal
J3_initial = q1 - q2
J3_offset = elbowStartFrame.M.GetRPY()[0] # J3's zero position is below horizontal
J3 = J3_initial - J3_offset
# 4. Complete (above)
# print("J2: {} J3: {}".format(J2, J3))
# 5. Use the Theta from cylindrical coordinates as the J1 angle, and update J5 accordingly
J1 = orange_Theta - math.radians(90)
J5 = J5_initial - (orange_Theta - math.radians(90))
# 5. Complete (above)
# print("J1: {} J5: {}".format(J1, J5))
jointAngles = [J1, J2, J3, J4, J5, J6]
jointNames = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll']
print("\n\nFinal joint angles in radians: {}\n\n".format(jointAngles))
in_bounds = True
for angle, name in zip(jointAngles, jointNames):
limit = robot_urdf.joint_map[name].limit
if angle < limit.lower or angle > limit.upper:
in_bounds = False
print("Joint {} out of bounds with angle {} not between {} and {}".format(name, angle, limit.lower, limit.upper))
print("All in bounds: {}".format(in_bounds))
solvedJoints = PyKDL.JntArray(8)
solvedJoints[0], solvedJoints[1], solvedJoints[3], solvedJoints[5], solvedJoints[6], solvedJoints[7] = jointAngles
solvedJoints[2], solvedJoints[4] = -1 * solvedJoints[1], -1 * solvedJoints[3]
producedFrame = PyKDL.Frame()
for i in range(chain.getNrOfSegments()):
rc = chainFK.JntToCart(solvedJoints, producedFrame, segmentNr=i)
plotter.addVector(producedFrame.p, "fk_produced_{}".format(i))
# print("Result: {}".format(rc))
# print("Output position: {}\nExpected position: {}".format(producedFrame.p, targetFrame.p))
# print("Output orientation: {}\nExpected orientation: {}".format(producedFrame.M, targetFrame.M))
# 6. (optional) Sanity check on solution:
# sanityTest(BASE_TO_BASE_YAW, BASE_YAW_TO_BOTTOM_4B, targetFrame, cameraFrame, cameraOffsetChain, jointAngles)
# 7. Create JointState message for return
ret = JointState()
ret.name = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll']
ret.header.stamp = rospy.Time.now()
ret.position = jointAngles
plotter.addGoal(ret)
# plotter.publish()
return in_bounds, ret
def handle(self):
self.socket_lock = threading.Lock()
self.last_joint_states = None
setConnectedRobot(self)
print "Handling a request"
try:
buf = self.recv_more()
if not buf: return
while True:
#print "Buf:", [ord(b) for b in buf]
# Unpacks the message type
mtype = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
#print "Message type:", mtype
if mtype == MSG_OUT:
# Unpacks string message, terminated by tilde
i = buf.find("~")
while i < 0:
buf = buf + self.recv_more()
i = buf.find("~")
if len(buf) > 2000:
raise Exception(
"Probably forgot to terminate a string: %s..."
% buf[:150])
s, buf = buf[:i], buf[i + 1:]
log("Out: %s" % s)
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3 * (6 * 4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!%ii" % (3 * 6), buf, 0)
buf = buf[3 * 6 * 4:]
state = [s / MULT_jointstate for s in state_mult]
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.name = joint_names
msg.position = [0.0] * 6
for i, q_meas in enumerate(state[:6]):
msg.position[i] = q_meas + joint_offsets.get(
joint_names[i], 0.0)
msg.velocity = state[6:12]
msg.effort = state[12:18]
self.last_joint_states = msg
pub_joint_states.publish(msg)
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF("Received quit")
elif mtype == MSG_WAYPOINT_FINISHED:
while len(buf) < 4:
buf = buf + self.recv_more()
waypoint_id = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
print "Waypoint finished (not handled)"
else:
raise Exception("Unknown message type: %i" % mtype)
if not buf:
buf = buf + self.recv_more()
except EOF, ex:
print "Connection closed (command):", ex
setConnectedRobot(None)
def main():
global iiwa0_reached
global iiwa0_received_desiredEEInRob
global iiwa0_desiredEEInRob_sent
global iiwa1_reached
global iiwa1_received_desiredEEInRob
global iiwa1_desiredEEInRob_sent
global exotica_complete
global iiwa0_desiredEEInRob
global iiwa1_desiredEEInRob
global data_man_tool
global iiwa0_currMsrTransform_sent
global iiwa1_currMsrTransform_sent
#Initialise ros
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('exotica_collision_detection', anonymous=True)
rate = rospy.Rate(10) # 10hz
#publisher
pub_iiwa0_destJoints = rospy.Publisher('iiwa0_destJoints',
JointState,
queue_size=10)
pub_iiwa0_destJoints_sent = rospy.Publisher('iiwa0_destJoints_sent',
Bool,
queue_size=10)
pub_iiwa0_reached = rospy.Publisher('iiwa0_reached', Bool, queue_size=10)
pub_iiwa1_destJoints = rospy.Publisher('iiwa1_destJoints',
JointState,
queue_size=10)
pub_iiwa1_destJoints_sent = rospy.Publisher('iiwa1_destJoints_sent',
Bool,
queue_size=10)
pub_iiwa1_reached = rospy.Publisher('iiwa1_reached', Bool, queue_size=10)
pub_exotica_complete = rospy.Publisher('exotica_complete',
Bool,
queue_size=10)
pub_iiwa0_desiredEEInRob_sent = rospy.Publisher(
'iiwa0_desiredEEInRob_sent', Bool, queue_size=10)
pub_iiwa1_desiredEEInRob_sent = rospy.Publisher(
'iiwa1_desiredEEInRob_sent', Bool, queue_size=10)
pub_iiwa0_currMsrTransform_sent = rospy.Publisher(
'iiwa0_currMsrTransform_sent', Bool, queue_size=10)
pub_iiwa1_currMsrTransform_sent = rospy.Publisher(
'iiwa1_currMsrTransform_sent', Bool, queue_size=10)
pub_iiwa0_currMsrTransform_received = rospy.Publisher(
'iiwa0_currMsrTransform_received', Bool, queue_size=10)
pub_iiwa1_currMsrTransform_received = rospy.Publisher(
'iiwa1_currMsrTransform_received', Bool, queue_size=10)
#subscriber
rospy.Subscriber("iiwa0_desiredEEInRob", Float64MultiArray,
callback_iiwa0_desiredEEInRob)
rospy.Subscriber("iiwa0_reached", Bool, callback_iiwa0_reached)
rospy.Subscriber("iiwa0_msrTransform", Float64MultiArray,
callback_iiwa0_msrTransform)
rospy.Subscriber("iiwa0_desiredEEInRob_sent", Bool,
callback_iiwa0_desiredEEInRob_sent)
rospy.Subscriber("iiwa1_desiredEEInRob", Float64MultiArray,
callback_iiwa1_desiredEEInRob)
rospy.Subscriber("iiwa1_reached", Bool, callback_iiwa1_reached)
rospy.Subscriber("iiwa1_msrTransform", Float64MultiArray,
callback_iiwa1_msrTransform)
rospy.Subscriber("iiwa1_desiredEEInRob_sent", Bool,
callback_iiwa1_desiredEEInRob_sent)
rospy.Subscriber("iiwa0_currMsrTransform_sent", Bool,
callback_iiwa0_currMsrTransform_sent)
rospy.Subscriber("iiwa1_currMsrTransform_sent", Bool,
callback_iiwa1_currMsrTransform_sent)
pub_exotica_complete.publish(True)
iiwa0_desiredEEInRob_sent = True ####################delete
iiwa1_desiredEEInRob_sent = False ####################delete
iiwa1_reached = True
####################delete
iiwa1_currMsrTransform_sent = True
####################delete
while (1):
#if (iiwa0_desiredEEInRob_sent or iiwa1_desiredEEInRob_sent) and iiwa0_currMsrTransform_sent and iiwa1_currMsrTransform_sent:
if (iiwa0_desiredEEInRob_sent or iiwa1_desiredEEInRob_sent
) and iiwa0_currMsrTransform_sent and iiwa1_currMsrTransform_sent:
print iiwa0_currMsrTransform_sent
iiwa0_currMsrTransform_sent = False
iiwa1_currMsrTransform_sent = False
pub_iiwa0_currMsrTransform_sent.publish(False)
pub_iiwa1_currMsrTransform_sent.publish(False)
pub_iiwa0_currMsrTransform_received.publish(True)
pub_iiwa1_currMsrTransform_received.publish(True)
print data_man_tool[0, 12:24]
#break;
pub_exotica_complete.publish(False)
if iiwa0_desiredEEInRob_sent:
#data_man_tool[1,12:24] = iiwa0_desiredEEInRob
data_man_tool[1, 12:24] = [
-4.800266e-01, -8.635465e-01, 1.544729e-01, 1.432567e-01,
2.227581e-01, -2.903022e-01, -9.306468e-01, -7.883426e-02,
8.485006e-01, -4.123251e-01, 3.317147e-01, 5.948348e-02
] ###########################delete
if iiwa1_desiredEEInRob_sent:
data_man_tool[1, 24:36] = iiwa1_desiredEEInRob
if not iiwa0_desiredEEInRob_sent:
data_man_tool[1, 12:24] = data_man_tool[0, 12:24]
if not iiwa1_desiredEEInRob_sent:
data_man_tool[1, 24:36] = data_man_tool[0, 24:36]
#print "iiwa0_desiredEEInRob"
#print data_man_tool[1,12:24]
#print "iiwa1_desiredEEInRob"
#print data_man_tool[1,24:36]
exo.Setup.initRos()
write_smoothed_traj(0, 0)
solver = exo.Setup.loadSolver(
'{stentgraft_sewing_planning}/resources/aico_trajectory.xml')
problem = solver.getProblem()
for t in range(0, problem.T):
problem.setRho('Frame0', 1e5, t)
problem.setRho('Frame1', 1e5, t)
solution = solver.solve()
if not is_trajectory_valid(problem, solution):
print 'Trajectory invalid'
save_trajectory(solution)
#print type(solution)
i = 0
print np.size(solution, 0)
print data_man_tool[0, 12:24]
break
##sending trajectories
while (i < np.size(solution, 0)):
if iiwa0_desiredEEInRob_sent:
msg0 = JointState()
msg0.name = [
"iiwa0 joint 1", "iiwa0 joint 2", "iiwa0 joint 3",
"iiwa0 joint 4", "iiwa0 joint 5", "iiwa0 joint 6",
"iiwa0 joint 7"
]
msg0.position = solution[i, 0:7]
#print msg0.position
pub_iiwa0_destJoints.publish(msg0)
if iiwa1_desiredEEInRob_sent:
msg1 = JointState()
msg1.name = [
"iiwa1 joint 1", "iiwa1 joint 2", "iiwa1 joint 3",
"iiwa1 joint 4", "iiwa1 joint 5", "iiwa1 joint 6",
"iiwa1 joint 7"
]
msg1.position = solution[i, 7:14]
#print msg1.position
pub_iiwa1_destJoints.publish(msg1)
rate.sleep()
#print i;
#print "iiwa0_reached ",
#print iiwa0_reached
#print "iiwa1_reached ",
#print iiwa1_reached
if (iiwa0_desiredEEInRob_sent and iiwa1_desiredEEInRob_sent
and iiwa0_reached and iiwa1_reached):
i = i + 1
iiwa0_reached = False
iiwa1_reached = False
pub_iiwa0_reached.publish(False)
pub_iiwa1_reached.publish(False)
if (iiwa0_desiredEEInRob_sent and not iiwa1_desiredEEInRob_sent
and iiwa0_reached and iiwa1_reached):
i = i + 1
iiwa0_reached = False
iiwa1_reached = True
pub_iiwa0_reached.publish(False)
pub_iiwa1_reached.publish(True)
if (not iiwa0_desiredEEInRob_sent and iiwa1_desiredEEInRob_sent
and iiwa0_reached and iiwa1_reached):
i = i + 1
iiwa0_reached = True
iiwa1_reached = False
pub_iiwa0_reached.publish(True)
pub_iiwa1_reached.publish(False)
pub_exotica_complete.publish(True)
iiwa0_desiredEEInRob_sent = False
iiwa1_desiredEEInRob_sent = False
pub_iiwa0_desiredEEInRob_sent.publish(False)
pub_iiwa1_desiredEEInRob_sent.publish(False)
def main(portName1, portBaud1):
print "JustinaHardwareLeftArm.->INITIALIZING LEFT ARM NODE BY MARCOSOFT..."
global objOnHand
global speedGripper
global torqueGripper
global gripperGoal_1
global gripperGoal_2
global torqueGripperCCW1
global torqueGripperCCW2
global gripperTorqueLimit
global gripperTorqueActive
global goalPos
global torqueMode
global speedsGoal
global newGoalPose
global armTorqueActive
global attemps
###Communication with dynamixels:
global dynMan1
print "JustinaHardwareLeftArm.->Trying to open port " + portName1 + " at " + str(portBaud1)
dynMan1 = Dynamixel.DynamixelMan(portName1, portBaud1)
msgCurrentPose = Float32MultiArray()
msgCurrentGripper = Float32()
msgBatery = Float32()
msgObjOnHand = Bool()
msgCurrentPose.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msgCurrentGripper.data = 0.0
msgBatery = 0.0
msgObjOnHand = False
curretPos = [0,0,0,0,0,0,0,0]
bitsPerRadian = (4095)/((360)*(3.141592/180))
bitValues = [0,0,0,0,0,0,0,0,0,0,0]
lastValues = [0,0,0,0,0,0,0,0,0,0,0]
goalPos = [0,0,0,0,0,0,0]
speedsGoal=[0, 0,0,0,0,0,0]
newGoalPose = False
objOnHand = False
torqueGripper = 0
currentLoad_D21 = 0
currentLoad_D22 = 0
posD21 = 0.0
posD22 = 0.0
gripperGoal_1 = zero_gripper[0]
gripperGoal_2 = zero_gripper[1]
attemps = 0
i = 0
##############################
### Connection with ROS ####
rospy.init_node("left_arm")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["la_1_joint", "la_2_joint", "la_3_joint", "la_4_joint", "la_5_joint", "la_6_joint", "la_7_joint", "la_grip_left", "la_grip_right"]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
subPos = rospy.Subscriber("/hardware/left_arm/goal_pose", Float32MultiArray, callbackArmPos)
subGripper = rospy.Subscriber("/hardware/left_arm/goal_gripper", Float32, callbackGripperPos)
subTorqueGripper = rospy.Subscriber("/hardware/left_arm/torque_gripper", Float32, callbackGripperTorque)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
pubArmPose = rospy.Publisher("left_arm/current_pose", Float32MultiArray, queue_size = 1)
pubGripper = rospy.Publisher("left_arm/current_gripper", Float32, queue_size = 1)
pubObjOnHand = rospy.Publisher("left_arm/object_on_hand", Bool, queue_size = 1)
pubBatery = rospy.Publisher("/hardware/robot_state/left_arm_battery", Float32, queue_size = 1)
#####################
## Dynamixel ##
for i in range(9):
#dynMan1.SetDGain(i, 25)
#dynMan1.SetPGain(i, 16)
#dynMan1.SetIGain(i, 6)
dynMan1.SetDGain(i, 0)
dynMan1.SetPGain(i, 32)
dynMan1.SetIGain(i, 0)
### Set servos features
for i in range(9):
dynMan1.SetMaxTorque(i, 1023)
dynMan1.SetTorqueLimit(i, 768)
dynMan1.SetHighestLimitTemperature(i, 80)
dynMan1.SetAlarmShutdown(i, 0b00000100)
#Dynamixel gripper on position Mode...
dynMan1.SetCWAngleLimit(7, 0)
dynMan1.SetCCWAngleLimit(7, 4095)
dynMan1.SetCWAngleLimit(8, 0)
dynMan1.SetCCWAngleLimit(8, 4095)
dynMan1.SetMovingSpeed(7, 100)
dynMan1.SetMovingSpeed(8, 100)
# Set torque_active for each servo
for i in range(7):
dynMan1.SetTorqueEnable(i, 1)
dynMan1.SetTorqueEnable(7, 1)
dynMan1.SetTorqueEnable(8, 1)
# Set initial pos for each servo
#for i in range(6):
# dynMan1.SetGoalPosition(i, zero_arm[i])
#dynMan1.SetGoalPosition(6, 2130)
#dynMan1.SetGoalPosition(7, zero_gripper[0])
#dynMan1.SetGoalPosition(8, zero_gripper[1])
loop = rospy.Rate(30)
while not rospy.is_shutdown():
#### Refresh arm_position ####
if newGoalPose:
newGoalPose = False
for i in range(7):
dynMan1.SetTorqueLimit(i, 768)
dynMan1.SetTorqueEnable(i, True)
dynMan1.SetMovingSpeed(i, speedsGoal[i])
dynMan1.SetGoalPosition(i, goalPos[i])
rospy.sleep(0.05)
#### Refresh gripper_pos ####
if attemps < 50:
if gripperTorqueActive :
dynMan1.SetCWAngleLimit(7, 0)
dynMan1.SetCCWAngleLimit(7, 0)
dynMan1.SetCWAngleLimit(8, 0)
dynMan1.SetCCWAngleLimit(8, 0)
dynMan1.SetTorqueLimit(7, torqueGripper)
dynMan1.SetTorqueLimit(8, torqueGripper)
dynMan1.SetTorqueVale(7, speedGripper, torqueGripperCCW1)
dynMan1.SetTorqueVale(8, speedGripper, torqueGripperCCW2)
currentLoad_D21 = dynMan1.GetPresentLoad(7)
currentLoad_D22 = dynMan1.GetPresentLoad(8)
else:
dynMan1.SetCWAngleLimit(7, 0)
dynMan1.SetCCWAngleLimit(7, 4095)
dynMan1.SetCWAngleLimit(8, 0)
dynMan1.SetCCWAngleLimit(8, 4095)
dynMan1.SetTorqueLimit(7, 500)
dynMan1.SetTorqueLimit(8, 500)
dynMan1.SetMovingSpeed(7, 200)
dynMan1.SetMovingSpeed(8, 200)
dynMan1.SetGoalPosition(7, gripperGoal_1)
dynMan1.SetGoalPosition(8, gripperGoal_2)
objOnHand = False
## This counter is reseated in the callback
attemps += 1
#### Refresh arms_position's readings #####
for i in range(9):
bitValues[i] = dynMan1.GetPresentPosition(i)
if(bitValues[i] == 0):
bitValues[i] = lastValues[i]
else:
lastValues[i] = bitValues[i]
# CurrentLoad > 1023 means the oposite sense of load
if currentLoad_D21 > 1023:
currentLoad_D21 -= 1023
if currentLoad_D22 > 1023:
currentLoad_D22 -= 1023
if gripperTorqueActive:
currentLoad_D21 = (currentLoad_D21 + currentLoad_D22)/2
if currentLoad_D21 > 200 and posD21 > -0.05 :
objOnHand = True
else:
objOnHand = False
#print "Load_left_gripper: " + str(currentLoad_D21)
#print "pos_left_gripper: " + str(posD21)
pos0 = float(-(zero_arm[0]-bitValues[0])/bitsPerRadian)
pos1 = float(-(zero_arm[1]-bitValues[1])/bitsPerRadian)
pos2 = float(-(zero_arm[2]-bitValues[2])/bitsPerRadian)
pos3 = float( (zero_arm[3]-bitValues[3])/bitsPerRadian)
pos4 = float(-(zero_arm[4]-bitValues[4])/bitsPerRadian)
pos5 = float(-(zero_arm[5]-bitValues[5])/bitsPerRadian)
pos6 = float(-(zero_arm[6]-bitValues[6])/bitsPerRadian)
posD21 = float(-(zero_gripper[0]-bitValues[7])/bitsPerRadian)
posD22 = float( (zero_gripper[1]-bitValues[8])/bitsPerRadian)
jointStates.header.stamp = rospy.Time.now()
jointStates.position[0] = pos0
jointStates.position[1] = pos1
jointStates.position[2] = pos2
jointStates.position[3] = pos3
jointStates.position[4] = pos4
jointStates.position[5] = pos5
jointStates.position[6] = pos6
jointStates.position[7] = posD21
jointStates.position[8] = posD22
msgCurrentPose.data[0] = pos0
msgCurrentPose.data[1] = pos1
msgCurrentPose.data[2] = pos2
msgCurrentPose.data[3] = pos3
msgCurrentPose.data[4] = pos4
msgCurrentPose.data[5] = pos5
msgCurrentPose.data[6] = pos6
msgCurrentGripper.data = posD21
msgObjOnHand = objOnHand
pubJointStates.publish(jointStates)
pubArmPose.publish(msgCurrentPose)
pubGripper.publish(msgCurrentGripper)
pubObjOnHand.publish(msgObjOnHand)
if i == 20:
msgBatery = float(dynMan1.GetPresentVoltage(2)/10.0)
pubBatery.publish(msgBatery)
i=0
i+=1
loop.sleep()
dynMan1.SetTorqueDisable(0)
dynMan1.SetTorqueDisable(1)
dynMan1.SetTorqueDisable(2)
dynMan1.SetTorqueDisable(3)
dynMan1.SetTorqueDisable(4)
dynMan1.SetTorqueDisable(5)
dynMan1.SetTorqueDisable(6)
dynMan1.Close()
from sensor_msgs.msg import JointState
from math import pi, sin
import rospy
topic = "joint_states"
publisher = rospy.Publisher(topic, JointState, queue_size=1)
rospy.init_node("send_efforts")
t = 0
msg = JointState()
msg.name = [
"panda_joint1",
"panda_joint2",
"panda_joint3",
"panda_joint4",
"panda_joint5",
"panda_joint6",
"panda_joint7",
"panda_finger_joint1",
]
N = len(msg.name)
msg.position = [0.0 for i in range(N)]
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
msg.effort = [10.0 * sin(t) for i in range(N)]
publisher.publish(msg)
t = t + 0.1
rospy.sleep(0.1)
def handleUpdateResponse(r):
"""Handles the response for an update request
Parses the message received from the robot, then publish the result
Arguments:
r {bytearray} -- response from the robot
Returns:
RobotState -- state object
"""
global last_state
global update_response
global stored_pose_count
global do_update
global p_joint_states
global p_robot_state
i = 0
# dropping the \0 from the end
r = r[:-1].split(" ")
try:
state = rosweld_drivers.msg.RobotState()
state.speed = int(r[i])
i = i + 1
state.pose.position.x = float(r[i]) / 1000
i = i + 1
state.pose.position.y = float(r[i]) / 1000
i = i + 1
state.pose.position.z = float(r[i]) / 1000
i = i + 1
state.euler_angles.rx = float(r[i])
i = i + 1
state.euler_angles.ry = float(r[i])
i = i + 1
state.euler_angles.rz = float(r[i])
i = i + 1
orientation = tf.transformations.quaternion_from_euler(
radians(state.euler_angles.rz),
radians(state.euler_angles.ry),
radians(state.euler_angles.rx),
axes)
state.pose.orientation.x = orientation[0]
state.pose.orientation.y = orientation[1]
state.pose.orientation.z = orientation[2]
state.pose.orientation.w = orientation[3]
# -1 from the step index, because Hyundai's list indexing is from 1, not zero
state.step = int(r[i]) - 1
i = i + 1
state.storedPoses = int(r[i])
i = i + 1
stateId = int(r[i])
state.robotProgramState = RobotStates.toString(stateId)
state.mode = "Not supported"
for _ in range(6):
i = i + 1
state.joints.append(float(r[i]))
update_response = True
state.isMoving = last_state is not None and last_state.pose != state.pose
if last_state != state or do_update:
js = JointState()
js.name = [
"joint_1",
"joint_2",
"joint_3",
"joint_4",
"joint_5",
"joint_6",
"joint_tip"]
js.position = [
state.joints[0],
-1 * (state.joints[1] - pi / 2),
-1 * state.joints[2],
state.joints[3],
-1 * state.joints[4],
state.joints[5],
0.0
]
p_joint_states.publish(js)
p_robot_state.publish(state)
do_update = False
last_state = state
stored_pose_count = state.storedPoses
except Exception as e:
global name
status(name, "Robot update failed: %s" % (str(e)), STATE.ERROR)
return state
rospy.wait_for_service('/compute_ik')
ik_solution = rospy.ServiceProxy('/compute_ik', GetPositionIK)
#
ref_frame = Header()
ref_frame.frame_id = "base_link"
ref_frame.stamp = rospy.Time.now()
#
# fk_link_names
fk_link_names = ["racket"]
joint_state = JointState()
joint_state.header = Header()
joint_state.header.frame_id = "racket"
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
]
joint_state.position = [0, 0, 0, 0, 0, 0]
robot_joint_state = RobotState()
robot_joint_state.joint_state = joint_state
position_1 = PoseStamped()
position_1.header = ref_frame
position_1.pose.position.x = 0.68
position_1.pose.position.y = 0.1
position_1.pose.position.z = 0.5
position_1.pose.orientation.x = 0
position_1.pose.orientation.y = 0
position_1.pose.orientation.z = 0
position_1.pose.orientation.w = 1
listener3 = rospy.Subscriber("pdcontroller_goal_simulation",
PDControllerGoal8,
callbackCurrentGoal,
queue_size=1) # necessary for LabelApp2
arm_id = rospy.get_param('arm_id')
jointStateMsgnames_suffixes = [
"_joint1", "_joint2", "_joint3", "_joint4", "_joint5", "_joint6",
"_joint7", "_finger_joint1", "_finger_joint2"
]
jointStateMsgnames = [arm_id + sfx for sfx in jointStateMsgnames_suffixes]
print(jointStateMsgnames)
j = JointState()
j.name = jointStateMsgnames
j.position = [0] * len(jointStateMsgnames)
j.velocity = [0] * len(jointStateMsgnames)
j.effort = [0] * len(jointStateMsgnames)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
rate.sleep()
if currentRobotState is not None:
j.position[0:7] = currentRobotState.q[0:7]
j.velocity[0:7] = currentRobotState.dq[0:7]
j.effort[0:7] = currentRobotState.tau[0:7]
j.position[7] = 0.5 * currentRobotState.q[7]
j.velocity[7] = 0.5 * currentRobotState.dq[7]
j.effort[7] = currentRobotState.tau[7]
j.position[8] = j.position[7]
else:
q[i] = acosr(D[:, i + 1].T @ vref)
if (D[:, i + 1].T @ v < 0): q[i] = -q[i]
if (np.abs(q[i]) > qlim[i]):
print('a junta', i + 1, 'excedeu')
return q
#configurando o Rviz
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(100) # 100hz
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['One_joint', 'Two_joint', 'Three_joint', 'Four_joint'\
,'Five_joint','Six_joint','Seven_joint','L_F_joint','R_F_joint']
hello_str.position = [0, 0, 0, 0, 0, 0, 0, 0, 0]
hello_str.velocity = []
hello_str.effort = []
n = 7 #numero de juntas
x = vetor([1, 0, 0])
y = vetor([0, 1, 0])
z = vetor([0, 0, 1])
#Gera uma pose aleatória desejada (posição + orientação)
posicaod, orientd = random_pose()
orientd2 = orientacao(orientd)
destino = posicaod
print('destino:', np.round(destino[:, 0], 3))
print('orientação', np.round(orientd2, 3))
if not rospy.is_shutdown() and _hold_position:
jointstate = JointState()
jointstate.name = hebi_wrap.hebi_mapping
jointstate.position = _hold_joint_position
jointstate.velocity = []
jointstate.effort = []
hebi_wrap.joint_state_publisher.publish(jointstate)
hebi_wrap.add_feedback_callback(_hold_position_cb)
# Main loop
while not rospy.is_shutdown():
# Get user input
valid_input = False
user_input = None
while not rospy.is_shutdown() and not valid_input:
print("\nPlease enter target position:")
user_input = float(raw_input())
valid_input = True
jointstate = JointState()
jointstate.name = hebi_wrap.hebi_mapping
jointstate.position = [user_input]
jointstate.velocity = []
jointstate.effort = []
_hold_position = False
hebi_wrap.joint_state_publisher.publish(jointstate)
_hold_joint_position = jointstate.position
_hold_position = True
def talker():
global getMode, get_last_vel, myflags
iter = 0
iter1 = 0
get_orientation = []
get_euler = []
get_matrix = []
get_velocity = []
get_invert = []
jointTorques = []
jointTorques1 = []
init_pos = []
middle_target = []
joint_Kp = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]
joint_Kd = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
stand_target = [0.0, -0.8, 1.6, 0.0, -0.8, 1.6, 0.0, -0.8, 1.6, 0.0, -0.8, 1.6]
pub1 = rospy.Publisher('/get_js', JointState, queue_size=100)
pub2 = rospy.Publisher('/imu_body', Imu, queue_size=100)
pub3 = rospy.Publisher('/get_com', commandDes, queue_size=100)
imu_msg = Imu()
setJSMsg = JointState()
com_msg = commandDes()
freq = 400
rate = rospy.Rate(freq) # hz
while not rospy.is_shutdown():
if myflags == 100:
p.resetBasePositionAndOrientation(boxId, [0, 0, 0.2], [0, 0, 0, 1])
time.sleep(1)
for j in range(16):
force = 0
p.setJointMotorControl2(boxId, j, p.VELOCITY_CONTROL, force=force)
get_orientation = []
pose_orn = p.getBasePositionAndOrientation(boxId)
for i in range(4):
get_orientation.append(pose_orn[1][i])
get_euler = p.getEulerFromQuaternion(get_orientation)
get_velocity = p.getBaseVelocity(boxId)
get_invert = p.invertTransform(pose_orn[0], pose_orn[1])
get_matrix = p.getMatrixFromQuaternion(get_invert[1])
# IMU data
imu_msg.orientation.x = pose_orn[1][0]
imu_msg.orientation.y = pose_orn[1][1]
imu_msg.orientation.z = pose_orn[1][2]
imu_msg.orientation.w = pose_orn[1][3]
imu_msg.angular_velocity.x = get_matrix[0] * get_velocity[1][0] + get_matrix[1] * get_velocity[1][1] + get_matrix[2] * get_velocity[1][2]
imu_msg.angular_velocity.y = get_matrix[3] * get_velocity[1][0] + get_matrix[4] * get_velocity[1][1] + get_matrix[5] * get_velocity[1][2]
imu_msg.angular_velocity.z = get_matrix[6] * get_velocity[1][0] + get_matrix[7] * get_velocity[1][1] + get_matrix[8] * get_velocity[1][2]
# calculate the acceleration of the robot
linear_X = (get_velocity[0][0] - get_last_vel[0]) * freq
linear_Y = (get_velocity[0][1] - get_last_vel[1]) * freq
linear_Z = 9.8 + (get_velocity[0][2] - get_last_vel[2]) * freq
imu_msg.linear_acceleration.x = get_matrix[0] * linear_X + get_matrix[1] * linear_Y + get_matrix[2] * linear_Z
imu_msg.linear_acceleration.y = get_matrix[3] * linear_X + get_matrix[4] * linear_Y + get_matrix[5] * linear_Z
imu_msg.linear_acceleration.z = get_matrix[6] * linear_X + get_matrix[7] * linear_Y + get_matrix[8] * linear_Z
# joint data
joint_state = p.getJointStates(boxId, motor_id_list)
setJSMsg.position = [joint_state[0][0], joint_state[1][0], joint_state[2][0],
joint_state[3][0], joint_state[4][0], joint_state[5][0],
joint_state[6][0], joint_state[7][0], joint_state[8][0],
joint_state[9][0], joint_state[10][0], joint_state[11][0]]
setJSMsg.velocity = [joint_state[0][1], joint_state[1][1], joint_state[2][1],
joint_state[3][1], joint_state[4][1], joint_state[5][1],
joint_state[6][1], joint_state[7][1], joint_state[8][1],
joint_state[9][1], joint_state[10][1], joint_state[11][1]]
# com data
com_msg.com_position = [pose_orn[0][0], pose_orn[0][1], pose_orn[0][2]]
com_msg.com_velocity = [get_velocity[0][0], get_velocity[0][1], get_velocity[0][2]]
get_last_vel.clear()
get_last_vel = com_msg.com_velocity
# stand up control
if len(get_effort):
print(get_effort)
p.setJointMotorControlArray(bodyUniqueId=boxId,
jointIndices=motor_id_list,
controlMode=p.TORQUE_CONTROL,
forces=get_effort)
setJSMsg.header.stamp = rospy.Time.now()
setJSMsg.name = ["abduct_fr", "thigh_fr", "knee_fr", "abduct_fl", "thigh_fl", "knee_fl",
"abduct_hr", "thigh_hr", "knee_hr", "abduct_hl", "thigh_hl", "knee_hl"]
if myflags % 2 == 0:
pub2.publish(imu_msg)
pub1.publish(setJSMsg)
pub3.publish(com_msg)
myflags = myflags + 1
p.setTimeStep(0.0015)
p.stepSimulation()
rate.sleep()
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
rospy.init_node("fake_wheel_pub")
pub = rospy.Publisher('joint_states', JointState, queue_size=5)
rate = 20
r = rospy.Rate(rate)
msg = JointState()
msg.name = ["left_wheel_joint", "right_wheel_joint"]
msg.position = [0.0 for name in msg.name]
msg.velocity = [0.0 for name in msg.name]
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
r.sleep()
def set_angles(self, angles):
self.check_module("direct_control_module")
msg = JointState()
msg.name = angles.keys()
msg.position = angles.values()
self._pub_joints.publish(msg)
def _command_to_state(self, joint_names, joint_values):
command_msg = JointState()
command_msg.header.stamp = rospy.get_rostime()
command_msg.name = joint_names
command_msg.position = joint_values
self._command_pub.publish(command_msg)
#!/usr/bin/env python
import roslib; roslib.load_manifest("cob_arm_navigation")
import rospy
from sensor_msgs.msg import JointState
rospy.init_node("tray_fake_pub")
p = rospy.Publisher('joint_states', JointState)
msg = JointState()
msg.name = ['tray_joint']
msg.position = [0.0 for name in msg.name]
msg.velocity = [0.0 for name in msg.name]
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
p.publish(msg)
rospy.sleep(0.1)
def talker():
pubHeadPitch = rospy.Publisher('/nao_dcm/HeadPitch_position_controller/command', Float64, queue_size=10)
pubHeadYaw = rospy.Publisher('/nao_dcm/HeadYaw_position_controller/command', Float64, queue_size=10)
pubLAnklePitch = rospy.Publisher('/nao_dcm/LAnklePitch_position_controller/command', Float64, queue_size=10)
pubLAnkleRoll = rospy.Publisher('/nao_dcm/LAnkleRoll_position_controller/command', Float64, queue_size=10)
pubLElbowRoll = rospy.Publisher('/nao_dcm/LElbowRoll_position_controller/command', Float64, queue_size=10)
pubLElbowYaw = rospy.Publisher('/nao_dcm/LElbowYaw_position_controller/command', Float64, queue_size=10)
pubLHand = rospy.Publisher('/nao_dcm/LHand_position_controller/command', Float64, queue_size=10)
pubLHipPitch = rospy.Publisher('/nao_dcm/LHipPitch_position_controller/command', Float64, queue_size=10)
pubLHipRoll = rospy.Publisher('/nao_dcm/LHipRoll_position_controller/command', Float64, queue_size=10)
pubLHipYawPitch = rospy.Publisher('/nao_dcm/LHipYawPitch_position_controller/command', Float64, queue_size=10)
pubLKneePitch = rospy.Publisher('/nao_dcm/LKneePitch_position_controller/command', Float64, queue_size=10)
pubLShoulderPitch = rospy.Publisher('/nao_dcm/LShoulderPitch_position_controller/command', Float64, queue_size=10)
pubLShoulderRoll = rospy.Publisher('/nao_dcm/LShoulderRoll_position_controller/command', Float64, queue_size=10)
pubLWristYaw = rospy.Publisher('/nao_dcm/LWristYaw_position_controller/command', Float64, queue_size=10)
pubRAnklePitch = rospy.Publisher('/nao_dcm/RAnklePitch_position_controller/command', Float64, queue_size=10)
pubRAnkleRoll = rospy.Publisher('/nao_dcm/RAnkleRoll_position_controller/command', Float64, queue_size=10)
pubRElbowRoll = rospy.Publisher('/nao_dcm/RElbowRoll_position_controller/command', Float64, queue_size=10)
pubRElbowYaw = rospy.Publisher('/nao_dcm/RElbowYaw_position_controller/command', Float64, queue_size=10)
pubRHand = rospy.Publisher('/nao_dcm/RHand_position_controller/command', Float64, queue_size=10)
pubRHipPitch = rospy.Publisher('/nao_dcm/RHipPitch_position_controller/command', Float64, queue_size=10)
pubRHipRoll = rospy.Publisher('/nao_dcm/RHipRoll_position_controller/command', Float64, queue_size=10)
pubRHipYawPitch = rospy.Publisher('/nao_dcm/RHipYawPitch_position_controller/command', Float64, queue_size=10)
pubRKneePitch = rospy.Publisher('/nao_dcm/RKneePitch_position_controller/command', Float64, queue_size=10)
pubRShoulderPitch = rospy.Publisher('/nao_dcm/RShoulderPitch_position_controller/command', Float64, queue_size=10)
pubRShoulderRoll = rospy.Publisher('/nao_dcm/RShoulderRoll_position_controller/command', Float64, queue_size=10)
pubRWristYaw = rospy.Publisher('/nao_dcm/RWristYaw_position_controller/command', Float64, queue_size=10)
rospy.init_node('jointPublisher')
rate = rospy.Rate(10) # 10hz
hello_str = JointState()
hello_str.header = Header()
listener = tf.TransformListener()
listener.waitForTransform("/head_1","/torso_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/left_hip_1","/torso_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/left_knee_1","/left_hip_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/left_foot_1","/left_knee_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/right_hip_1","/torso_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/right_knee_1","/right_hip_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/right_foot_1","/right_knee_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/left_shoulder_1","/torso_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/left_elbow_1","/left_shoulder_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/left_hand_1","/left_elbow_1",rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/right_hand_1","/right_elbow_1",rospy.Time(), rospy.Duration(4.0))
thetaLPitchO = thetaLRollO = thetaRPitchO = thetaRRollO = 0
thetaLHipPitchO = thetaRHipPitchO = 0
thetaLHipRollO = thetaRHipRollO = 0
thetaLKneeO = thetaRKneeO = 0
thetaLAnklePitchO = thetaRAnklePitchO = 0
thetaLAnkleRollO = thetaRAnkleRollO = 0
HipPitchMin = -1.535889
HipPitchMax = 0.48409
HipRollMin = -0.379472
HipRollMax = 0.790477
KneeMin = -0.092346
KneeMax = 2.112528
AnklePitchMin = -1.189516
AnklePitchMax = 0.922747
AnkleRollMin = -0.397880
AnkleRollMax = 0.769001
while not rospy.is_shutdown():
hello_str.header.stamp = rospy.Time.now()
now = rospy.Time.now()
listener.waitForTransform("/head_1","/torso_1",now,rospy.Duration(4.0))
(transHead, rotHead) = listener.lookupTransform("/head_1", "/torso_1", now)
eulerHead = tf.transformations.euler_from_quaternion(rotHead)
listener.waitForTransform("/left_hip_1","/torso_1",now,rospy.Duration(4.0))
(transLHip, rotLHip) = listener.lookupTransform('/left_hip_1','/torso_1',now)
eulerLHip = tf.transformations.euler_from_quaternion(rotLHip)
listener.waitForTransform("/left_knee_1","/left_hip_1",now,rospy.Duration(4.0))
(transLKnee, rotLKnee) = listener.lookupTransform('/left_knee_1','/left_hip_1',now)
eulerLKnee = tf.transformations.euler_from_quaternion(rotLKnee)
listener.waitForTransform("/left_foot_1","/left_knee_1",now,rospy.Duration(4.0))
(transLAnkle, rotLAnkle) = listener.lookupTransform('/left_foot_1','/left_knee_1',now)
eulerLAnkle = tf.transformations.euler_from_quaternion(rotLAnkle)
listener.waitForTransform("/right_hip_1","/torso_1",now,rospy.Duration(4.0))
(transRHip, rotRHip) = listener.lookupTransform('/right_hip_1','/torso_1',now)
eulerRHip = tf.transformations.euler_from_quaternion(rotRHip)
listener.waitForTransform("/right_knee_1","/right_hip_1",now,rospy.Duration(4.0))
(transRKnee, rotRKnee) = listener.lookupTransform('/right_knee_1','/right_hip_1',now)
eulerRKnee = tf.transformations.euler_from_quaternion(rotRKnee)
listener.waitForTransform("/right_foot_1","/right_knee_1",now,rospy.Duration(4.0))
(transRAnkle, rotRAnkle) = listener.lookupTransform('/right_foot_1','/right_knee_1',now)
eulerRAnkle = tf.transformations.euler_from_quaternion(rotRAnkle)
listener.waitForTransform("/left_shoulder_1","/torso_1",now,rospy.Duration(4.0))
(transLShoulder, rotLShoulder) = listener.lookupTransform('/left_shoulder_1','/torso_1',now)
eulerLShoulder = tf.transformations.euler_from_quaternion(rotLShoulder)
listener.waitForTransform("/left_elbow_1","/left_shoulder_1",now,rospy.Duration(4.0))
(transLElbow, rotLElbow) = listener.lookupTransform('/left_elbow_1','/left_shoulder_1',now)
eulerLElbow = tf.transformations.euler_from_quaternion(rotLElbow)
listener.waitForTransform("/left_hand_1","/left_elbow_1",now,rospy.Duration(4.0))
(transLWrist, rotLWrist) = listener.lookupTransform('/left_hand_1','/left_elbow_1',now)
eulerLWrist = tf.transformations.euler_from_quaternion(rotLWrist)
listener.waitForTransform("/right_shoulder_1","/torso_1",now,rospy.Duration(4.0))
(transRShoulder, rotRShoulder) = listener.lookupTransform('/right_shoulder_1','/torso_1',now)
eulerRShoulder = tf.transformations.euler_from_quaternion(rotRShoulder)
listener.waitForTransform("/right_elbow_1","/right_shoulder_1",now,rospy.Duration(4.0))
(transRElbow, rotRElbow) = listener.lookupTransform('/right_elbow_1','/right_shoulder_1',now)
eulerRElbow = tf.transformations.euler_from_quaternion(rotRElbow)
listener.waitForTransform("/right_hand_1","/right_elbow_1",now,rospy.Duration(4.0))
(transRWrist, rotRWrist) = listener.lookupTransform('/right_hand_1','/right_elbow_1',now)
eulerRWrist = tf.transformations.euler_from_quaternion(rotRWrist)
hello_str.name = ['HeadYaw', 'HeadPitch', 'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'LHand', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'RHand', 'RFinger23', 'RFinger13', 'RFinger12', 'LFinger21', 'LFinger13', 'LFinger11', 'RFinger22', 'LFinger22', 'RFinger21', 'LFinger12', 'RFinger11', 'LFinger23', 'LThumb1', 'RThumb1', 'RThumb2', 'LThumb2']
hello_str.position = [eulerHead[2] , eulerHead[1],0.0,-eulerLHip[2], eulerLHip[0], eulerLKnee[0],eulerLAnkle[1] , eulerLAnkle[0], 0.0, -eulerRHip[2], eulerRHip[0], eulerRKnee[0], eulerRAnkle[1], eulerRAnkle[0], eulerLShoulder[0] + math.pi/2, -eulerLShoulder[2] + math.pi/2,eulerLElbow[0]-math.pi/2, eulerLElbow[1], eulerLWrist[2], 0.0, eulerRShoulder[0] + math.pi/2, -eulerRShoulder[2] - math.pi/2,-eulerRElbow[0]+math.pi/2, eulerRElbow[1], eulerRWrist[2], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
hello_str.velocity = []
hello_str.effort = []
#parameters
roLHipPitch = hello_str.position[4]
roLHipRoll = hello_str.position[3]
roRHipPitch = hello_str.position[10]
roRHipRoll = hello_str.position[9]
roLKnee = hello_str.position[5]
roRKnee = hello_str.position[11]
roLShoulderPitch = hello_str.position[14]
roLShoulderRoll = hello_str.position[15]
roRShoulderPitch = hello_str.position[20]
roRShoulderRoll = hello_str.position[21]
roLElbowYaw = hello_str.position[16]
roLElbowRoll = hello_str.position[17]
roRElbowYaw = hello_str.position[22]
roRElbowRoll = hello_str.position[23]
ThetaL = np.array([roLHipPitch,roLHipRoll,roRHipPitch,roRHipRoll,roLKnee,roRKnee])
ThetaU = np.array([roLShoulderPitch, roLShoulderRoll, roRShoulderPitch, roRShoulderRoll,
roLElbowYaw, roLElbowRoll, roRElbowYaw, roRElbowRoll])
result = calcAnkleLine(ThetaL, ThetaU)
#result = judge(result,ThetaL, ThetaU)
thetaLPitch = result[3]
thetaLRoll = result[4]
thetaRPitch = result[5]
thetaRRoll = result[6]
#define restrict for joint angles
#http://doc.aldebaran.com/1-14/family/robots/joints_robot.html#robot-joints-v4-left-leg-joints
if roLHipPitch < HipPitchMin or roLHipPitch > HipPitchMax:
roLHipPitch = thetaLHipPitchO
else:
thetaLHipPitchO = roLHipPitch
if roRHipPitch < HipPitchMin or roRHipPitch > HipPitchMax:
roRHipPitch = thetaRHipPitchO
else:
thetaRHipPitchO = roRHipPitch
if roLHipRoll < HipRollMin or roLHipRoll > HipRollMax:
roLHipRoll = thetaLHipRollO
else:
thetaLHipRollO = roLHipRoll
if roRHipRoll < HipRollMin or roRHipRoll > HipRollMax:
roRHipRoll = thetaRHipRollO
else:
thetaRHipRollO = roRHipRoll
if roLKnee < KneeMin or roLKnee > KneeMax:
roLKnee = thetaLKneeO
else:
thetaLKneeO = roLKnee
if roRKnee < KneeMin or roRKnee > KneeMax:
roRKnee = thetaRKneeO
else:
thetaRKneeO = roRKnee
#if ankle angle exceeds the limit, restore all angles
if thetaLPitch < AnklePitchMin or thetaLPitch > AnklePitchMax \
or thetaRPitch < AnklePitchMin or thetaRPitch > AnklePitchMax \
or thetaLRoll < AnkleRollMin or thetaLRoll > AnkleRollMax \
or thetaRRoll < AnkleRollMin or thetaRRoll > AnkleRollMax:
thetaLPitch = thetaLAnklePitchO
thetaRPitch = thetaRAnklePitchO
thetaLRoll = thetaLAnkleRollO
thetaRRoll = thetaRAnkleRollO
roLHipPitch = thetaLHipPitchO
roRHipPitch = thetaRHipPitchO
roLHipRoll = thetaLHipRollO
roRHipRoll = thetaRHipRollO
roLKnee = thetaLKneeO
roRKnee = thetaRKneeO
else:
thetaLAnklePitchO = thetaLPitch
thetaRAnklePitchO = thetaRPitch
thetaLAnkleRollO = thetaLRoll
thetaRAnkleRollO = thetaRRoll
print("thetaLPitch, thetaLRoll, thetaPitch, thetaRRoll: ",thetaLPitch, thetaLRoll, thetaRPitch, thetaRRoll)
#pubHeadPitch.publish(hello_str.position[1])
#pubHeadYaw.publish(hello_str.position[0])
pubLAnklePitch.publish(thetaLPitch)
pubLAnkleRoll.publish(thetaLRoll)
pubLElbowRoll.publish(hello_str.position[17])
pubLElbowYaw.publish(hello_str.position[16])
#pubLHand.publish(hello_str.position[19])
pubLHipPitch.publish(roLHipPitch)
pubLHipRoll.publish(roLHipRoll)
#pubLHipYawPitch.publish(hello_str.position[2])
pubLKneePitch.publish(roLKnee)
pubLShoulderPitch.publish(hello_str.position[14])
pubLShoulderRoll.publish(hello_str.position[15])
#pubLWristYaw.publish(hello_str.position[18])
pubRAnklePitch.publish(thetaRPitch)
pubRAnkleRoll.publish(thetaRRoll)
pubRElbowRoll.publish(hello_str.position[23])
pubRElbowYaw.publish(hello_str.position[22])
#pubRHand.publish(hello_str.position[25])
pubRHipPitch.publish(roRHipPitch)
pubRHipRoll.publish(roRHipRoll)
#pubRHipYawPitch.publish(hello_str.position[8])
pubRKneePitch.publish(roRKnee)
pubRShoulderPitch.publish(hello_str.position[20])
pubRShoulderRoll.publish(hello_str.position[21])
#pubRWristYaw.publish(hello_str.position[24])
rate.sleep()
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10)
rospy.init_node('display', anonymous=True)
rate = rospy.Rate(30) # 30hz
# pub joint state
joint_state_send = JointState()
joint_state_send.header = Header()
joint_state_send.name = [
'joint2_to_joint1',
'joint3_to_joint2',
'joint4_to_joint3',
'joint5_to_joint4',
'joint6_to_joint5',
'joint6output_to_joint6'
]
joint_state_send.velocity = [0]
joint_state_send.effort = []
marker_ = Marker()
marker_.header.frame_id = '/joint1'
marker_.ns = 'my_namespace'
while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now()
angles = mycobot.get_angles_of_radian()
data_list = []
for index, value in enumerate(angles):
if index != 2:
value *= -1
data_list.append(value)
joint_state_send.position = data_list
pub.publish(joint_state_send)
coords = mycobot.get_coords()
rospy.loginfo('{}'.format(coords))
#marker
marker_.header.stamp = rospy.Time.now()
marker_.type = marker_.SPHERE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
#marker position initial
# print(coords)
if not coords:
coords = [0,0,0,0,0,0]
rospy.loginfo('error [101]: can not get coord values')
marker_.pose.position.x = coords[1] / 1000 * -1
marker_.pose.position.y = coords[0] / 1000
marker_.pose.position.z = coords[2] / 1000
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
def timer_callback(self):
phase = fract(self.t / self.period)
transfer_time = 1.0 - self.beta
base_x = 0.03
y = 0.09 # relative to leg
base_z = -0.07
names = []
positions = []
for i in range(4):
changed = False
dx = 0.0
leg = self.leg_mapping[i]
front, side = leg.split('_')
x_sign = 1. if front == 'front' else -1.
y_sign = 1. if side == 'left' else -1.
leg_phase = phase - self.phi[i]
if leg_phase < 0.0:
leg_phase += 1.0
if leg_phase >= self.beta: # transfer
changed = True
transfer_phase = \
(leg_phase - self.beta) / (1 - self.beta)
x = x_sign * base_x + (-self.R / 2.) + self.R * transfer_phase
z = base_z + 0.05 * sin(transfer_phase * pi)
elif (0.0 <= phase and phase <= (0.5 - 2 * transfer_time)): # move
changed = True
move_phase = phase / (0.5 - 2 * transfer_time)
dx = (self.R / 2.) * move_phase
x = \
x_sign * base_x + \
((side == 'left') * (self.R / 2.)) - \
dx
z = base_z # relative to leg
elif (0.5 <= phase and phase <= (1.0 - 2 * transfer_time)): # move
changed = True
move_phase = (phase - 0.5) / (0.5 - 2 * transfer_time)
dx = (self.R / 2.) * move_phase
x = \
x_sign * base_x + \
((side == 'right') * (self.R / 2.)) - \
dx
z = base_z # relative to leg
if changed:
req = LegInvKin.Request()
req.x = x
req.y = y * y_sign
req.z = z
future = self.inv_kin_cli.call_async(req)
rclpy.spin_until_future_complete(self, future)
res = future.result()
if res is None or not res.success:
print('leg_inv_kin error')
continue
a, b, g = res.angles
names += [
f'{leg}_base_to_{leg}_link1',
f'{leg}_link1_to_{leg}_link2',
f'{leg}_link2_to_{leg}_link3'
]
positions += [a, b, g]
self.transform.transform.translation.x += \
dx * self.timer_period
msg = JointState()
msg.name = names
msg.position = positions
self.pub.publish(msg)
msg = TFMessage(transforms=[self.transform])
self.broadcaster.publish(msg)
self.t += self.timer_period
target_linear_vel = 0.0
target_angular_vel = 0.0
control_linear_vel = 0.0
control_angular_vel = 0.0
servox = 0
servoy = 0
try:
print(msg)
while (1):
key = getKey()
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['joint1', 'joint2']
hello_str.position = [servox, servoy]
hello_str.velocity = []
hello_str.effort = []
pub1.publish(hello_str)
if (servox < -1.5):
servox = -1.5
elif (servox > 1.5):
servox = 1.5
if (servoy < -0.45):
servoy = -0.45
elif (servoy > 1.5):
servoy = 1.5
if key == 'w':
target_linear_vel = checkLinearLimitVelocity(
def flex_ik_service_client(self,
i_Limb="right",
i_Pose=None,
i_UseAdvanced=False):
if self.is_adding_new_item == True: # Can't move if user is in close proximity manually moving the arm
return False
if i_Pose == None:
rospy.logerr(
"Error: flex_ik_service_client received 'None' in arg 'i_Pose' for target position"
)
return False
# Initialize IK service
service_name = "ExternalTools/" + i_Limb + "/PositionKinematicsNode/IKService"
ik_ServiceClient = rospy.ServiceProxy(
service_name, SolvePositionIK
) # Creates a handle to some service that we want to invoke methods on, in this case the SolvePositionIK
ik_ServiceReq = SolvePositionIKRequest(
) # Create a request message that will give us the inverse kinematics for some set of joints (Note that this is done at compile time)
msg_header = Header(
stamp=rospy.Time.now(), frame_id='base'
) # Generates a Header for the message (Think of HTTP)
# Hover over the object
hover_pose = copy.deepcopy(i_Pose)
hover_pose.position.z = hover_pose.position.z + self.hover_dist # hover over the object
# Set the goal positions for the limb that we specified (I'm pretty sure that it's for the last joint right_j6)
# Set the header that we time stamped
# Add Pose object that contains a Point and Quaternion
goal_positions = {
'right': PoseStamped(header=msg_header, pose=hover_pose),
}
##############################################################################################
#
# SOLVER IS SETUP TO FIND PATH USING SIMPLE INVERSE KINEMATICS
#
##############################################################################################
# Add the goal positions for the inverse kinematics
ik_ServiceReq.pose_stamp.append(goal_positions[i_Limb])
# Request the inverse kinematics from the base to the "right_hand" link
ik_ServiceReq.tip_names.append('right_hand')
##############################################################################################
#
# SETUP THE SOLVER TO FIND A PATH USING ADVANCED INVERSE KINEMATICS
#
##############################################################################################
if (i_UseAdvanced):
rospy.loginfo("Using Advanced IK Service")
# Define the joint seed. If the solver encounters the specified value for a joint, the it will attempt to do some optimisation
ik_ServiceReq.seed_mode = ik_ServiceReq.SEED_USER
seed = JointState(
) # JointState describes the state of each joint (possition, velocity, effort)
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
] # The name of the various joints
seed.position = [
0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4
] # The joint angle at which the solver tries to do optimisation for the respective joints
# Pass the seeded angles to the Solver via the SolvePositionIKRequest object
ik_ServiceReq.seed_angles.append(seed)
# NOTE Once the Primary IK Task has been solved, the IK Solver will try to bias the joint angles towards the goal joint configuration
# NOTE The null space represents the extra degrees of freedom that the joints can move through without affecting the results of the Primary IK Task
# Here we try to solve the Primary IK Task
ik_ServiceReq.use_nullspace_goal.append(True)
# NOTE The null space can either be a subset or the full set of joint angles
goal = JointState()
# This is the subset of joints that make up the null space "Joints A, B, C ... can try to move, but can't affect the result"
goal.name = ['right_j1', 'right_j2', 'right_j3']
goal.position = [
0.1, -0.3, 0.5
] # If these possitions are encountered, try to do some optimisation
ik_ServiceReq.nullspace_goal.append(goal)
ik_ServiceReq.nullspace_gain.append(0.4)
##############################################################################################
#
# PASS THE INVERSE KINEMATICS REQUEST TO THE SOLVER TO GET AN ACTUAL PATH FOR THE ROBOT
#
##############################################################################################
# rospy.loginfo("Simple IKService Solver Running...")
try:
rospy.wait_for_service(
service_name, timeout=self.arm_timeout
) # Waits for the service (5 seconds), creates the service if it doesn't already exist
response = ik_ServiceClient(
ik_ServiceReq
) # Get the response from the client, contains the joint positions
except (rospy.ServiceException, rospy.ROSException), ex:
rospy.logerr("Service Call Failed: %s" % (ex, ))
return False
rospy.get_param('/rotational_limits_joint_34')))
rotlim_45 = np.radians(np.array(
rospy.get_param('/rotational_limits_joint_45')))
rotlim_56 = np.radians(np.array(
rospy.get_param('/rotational_limits_joint_56')))
# Create the publisher. Name the topic "joint_angles_desired", with message type "JointState"
pub_joint_angles_desired = rospy.Publisher('/joint_angles_desired',
JointState,
queue_size=1)
# Create the message
cmds = [0., -np.pi / 2., np.pi / 2., 0., 0., 0.]
joint_angles_desired_msg = JointState()
joint_angles_desired_msg.name = [
'base_joint', 'shoulder_joint', 'elbow_joint', 'forearm_joint',
'wrist_joint', 'fingers_joint'
]
joint_angles_desired_msg.position = cmds # upright neutral position
def manual_joint_angles():
rospy.init_node('manual_joint_angles_node', anonymous=False)
while not rospy.is_shutdown():
try:
cmds = np.array(
input(
'\n Enter Joint angles list (comma separated, no brackets). Ctrl-C and Enter to exit.\nalpha0, beta1, beta2, gamma3, beta4, gamma5:\n'
))
def getJointStateCommand(self, time):
cmd = JointState()
cmd.name = self.names
# find previous which leg we are on
if (time < 0 or time > self.times[-1]):
return null
index = -1
for i in range(0, len(self.times) - 1):
if (self.times[i] <= time and time <= self.times[i + 1]):
index = i
break
if (index == -1):
return null
cmd.position = list([0, 0, 0, 0])
cmd.velocity = list([0, 0, 0, 0])
cmd.effort = list([NaN, NaN, NaN, NaN])
epsilon = 0.001
leg_duration = self.times[index + 1] - self.times[index]
leg_percentage = 1.0 - ((self.times[index + 1] - time) / leg_duration)
leg_percentage_epsilon = 1.0 - ((self.times[index + 1] -
(time + epsilon)) / leg_duration)
print leg_percentage
print leg_percentage_epsilon
# Calculate current workspace waypoint
wayp = list([0, 0, 0, 0, 0])
wayp_epsilon = list([0, 0, 0, 0, 0])
prev_wayp = \
[self.waypoints[0][index],
self.waypoints[1][index],
self.waypoints[2][index],
self.waypoints[3][index],
self.waypoints[4][index]]
next_wayp = \
[self.waypoints[0][index+1],
self.waypoints[1][index+1],
self.waypoints[2][index+1],
self.waypoints[3][index+1],
self.waypoints[4][index+1]]
for dof in range(0, 5):
difference = next_wayp[dof] - prev_wayp[dof]
wayp[dof] = prev_wayp[dof] + (leg_percentage * difference)
wayp_epsilon[dof] = prev_wayp[dof] + (leg_percentage_epsilon *
difference)
print wayp
print wayp_epsilon
cmd.position = kin.ik(wayp, self.elbow_up[index])
position_epsilon = kin.ik(wayp_epsilon, self.elbow_up[index])
for joint in range(0, 4):
cmd.velocity[joint] = (cmd.position[joint] -
position_epsilon[joint]) / epsilon
# Calculate current joint velocity)
# Calculate workspace waypoint a small time in the future
wayp_epsilon = list([0, 0, 0, 0, 0])
for dof in range(0, 5):
difference = next_wayp[dof] - prev_wayp[dof]
wayp[dof] = prev_wayp[dof] + (leg_percentage * leg_duration)
return cmd
def joint_state_define(x):
js = JointState()
js.name = joint_ordering
js.position = tuple(x)
return js
pub.publish(msg.position[idx])
# return True
return self.__play_motion_publisher.publish(msg)
def set_joint(self, msg):
self.__play_motion_publisher.publish(msg)
def cur_Joint_handler(self, msg):
pass
def subscribe(self, service):
self.__service = service
def unsubscribe(self, service):
self.__service = None
if __name__ == '__main__':
rospy.init_node('social_motion_player_service_test')
service = Service()
time.sleep(2)
msg = JointState()
msg.name = [
'LShoulder_Pitch', 'LShoulder_Roll', 'LElbow_Pitch', 'LElbow_Yaw',
'LWrist_Pitch', 'LFinger', 'RShoulder_Pitch', 'RShoulder_Roll',
'RElbow_Pitch', 'RElbow_Yaw', 'RWrist_Pitch', 'RFinger'
]
msg.position = [0, -1.221, 0, 0, 0, 0, 0, 1.221, 0, 0, 0, 0]
service.set_joint(msg)
