def calcLegPoseLF(self):
try:
self.thetas_lf = IK.legIK(np.linalg.inv(self.Tlf) @ self.Llf)
self.Llf_ex = self.Llf
except ValueError:
self.thetas_lf = IK.legIK(np.linalg.inv(self.Tlf) @ self.Llf_ex)
print("Left front leg uses except point")
def calcLegPoseLB(self):
try:
self.thetas_lb = IK.legIK(np.linalg.inv(self.Tlb) @ self.Llb)
self.Llb_ex = self.Llb
except ValueError:
self.thetas_lb = IK.legIK(np.linalg.inv(self.Tlb) @ self.Llb_ex)
print("Left back leg uses except point")
def calcLegPoseRF(self):
try:
self.thetas_rf = IK.legIK(
self.Ix @ np.linalg.inv(self.Trf) @ self.Lrf)
self.Lrf_ex = self.Lrf
except ValueError:
self.thetas_rf = IK.legIK(
self.Ix @ np.linalg.inv(self.Trf) @ self.Lrf_ex)
print("Right front leg uses except point")
def calcLegPoseRB(self):
try:
self.thetas_rb = IK.legIK(
self.Ix @ np.linalg.inv(self.Trb) @ self.Lrb)
self.Lrb_ex = self.Lrb
except ValueError:
self.thetas_rb = IK.legIK(
self.Ix @ np.linalg.inv(self.Trb) @ self.Lrb_ex)
print("Right back leg uses except point")
def SetIdlePos(kematox, mode):
"""Default value for idle stance is "D"=275, "z"=48"""
if mode == "set":
IK.IK_SixLeg()
kematox.MoveSixLeg(1000, "support")
elif mode == "return":
"""STEP 1 - TRIPOD A lift legs"""
IK.IK_in["POS_Z"] = -20.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 2 - TRIPOD A lower leg to idle D position (275)"""
IK.IK_in["D"] = 275.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 3 - TRIPOD B lift legs"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = -30.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 4 - TRIPOD B lower leg to idle D position (275)"""
IK.IK_in["D"] = 275.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 5 - lower body to idle position (z=48) """
IK.IK_in["z"] = 48.0
IK.IK_SixLeg()
kematox.MoveSixLeg(1500, "support")
def go(pose_target):
ik = IK.IK()
success, joints = ik.ik_service_client(pose_target, rospy)
limb_joints = dict(zip(joints.joints[0].name, joints.joints[0].position))
limb = intera_interface.Limb("right")
limb.set_joint_position_speed(.2)
limb.move_to_joint_positions(
limb_joints,
timeout=20.0,
threshold=intera_interface.settings.JOINT_ANGLE_TOLERANCE)
def move(self, message):
self.head_display.display_image("/home/microshak/Pictures/Moving.png",
False, 1.0)
print "MOVING!!!!!!!!!!!!!!!!!"
print(message)
position = ast.literal_eval(json.dumps(message['Cartisian']))
speed = message['Speed']
print(position)
p = position["position"]
o = position["orientation"]
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = o["w"]
pose_target.orientation.x = o["x"]
pose_target.orientation.y = o["y"]
pose_target.orientation.z = o["z"]
pose_target.position.x = p["x"]
pose_target.position.y = p["y"]
pose_target.position.z = p["z"]
if (speed == '' or speed == '1'):
print pose_target
group = moveit_commander.MoveGroupCommander("right_arm")
group.set_pose_target(pose_target)
plan2 = group.plan()
group.go(wait=True)
else:
ik = IK.IK()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose_stamp = geometry_msgs.msg.PoseStamped()
pose_stamp.pose = pose_target
success, joints = ik.ik_service_client(pose_stamp, rospy)
limb_joints = dict(
zip(joints.joints[0].name, joints.joints[0].position))
limb = intera_interface.Limb("right")
limb.set_joint_position_speed(float(speed))
limb.move_to_joint_positions(
limb_joints,
timeout=20.0,
threshold=intera_interface.settings.JOINT_ANGLE_TOLERANCE)
'''
def __init__(self, L_legs_start):
self.Llf = L_legs_start[0]
self.Lrf = L_legs_start[1]
self.Llb = L_legs_start[2]
self.Lrb = L_legs_start[3]
self.Llf_ex = np.array([100, 100, -100, 1]) # leg_points[0]
self.Lrf_ex = np.array([100, -100, -100, 1]) # leg_points[1]
self.Llb_ex = np.array([-100, 100, -100, 1]) # leg_points[2]
self.Lrb_ex = np.array([-100, -100, -100, 1]) # leg_points[3]
self.thetas_lf = []
self.thetas_rf = []
self.thetas_lb = []
self.thetas_rb = []
self.thetas = []
self.Ix = np.array([[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
(self.Tlf, self.Trf, self.Tlb, self.Trb) = IK.bodyIK(0, 0, 0, 0, 0, 0)
def CenterHead(kematox):
IK.CalcHeadPos(HeadMovInput, HeadCalibrVal, HeadMovOutput)
kematox.MoveHead(HeadMovOutput, 500)
def TripodWalk(kematox, walkval):
""" Tripod walkpattern """
def defineStepHeight():
""" check POS_Z value to deifne step high. In idle pos no walking allowed,
if POS_Z is below or equal with the half of max. POS_Z value than 25mm stepHigh is allowed
if POS_Z is more than than the half of the alloewed value than 50mm stephigh is allowed """
z_saved = 0.0 # Z pozició mentésére szolgáló változó. Az emelés után a láb ehhez a z-hez tartozó magasságba áljon vissza.
if IK.IK_in["POS_Z"] == 0:
stepHeight = 0.0
return stepHeight
elif IK.IK_in[
"POS_Z"] <= 65: # Az analog érétkek kerekítettek (-1 és 1 között 0,1-es lépésekben) ->meghatározott
stepHeight = -10.0 # értéket vehet fel Z. ->az if feltételeket ehhez kell igazítani. Az 51 azért nem
return stepHeight # müködött, mert ilyen értéket nem vehet fel Z ->mindig a nagyobb lépés teljesült...
elif IK.IK_in[
"POS_Z"] > 65: # z lehetséges érétkei a pos_Hitec_to_JX.xls-ben találhatóak.
stepHeight = -20.0
return stepHeight
if walkval["tripod_step_1_complete"] == False:
# 1. TRIPOD A support body and translates
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "support", 750)
# 2. TRIPOD B swigns -> raise and center TRIPOD B
z_saved = IK.IK_in["POS_Z"]
y_saved = IK.IK_in["POS_Y"]
IK.IK_in["POS_Z"] = defineStepHeight()
IK.IK_in["POS_X"] = 0.0
IK.IK_in["POS_Y"] = 0.0
IK.IK_in["ROT_Z"] = 0.0
IK.IK_Tripod_B(
"support"
) # !!! swing-nél a még a régi IK dict van használatban !!!
kematox.MoveTripodB("default", "swing", 750)
# 3. TRIPOD B swings -> lowering TRIPOD B
IK.IK_in["POS_Z"] = z_saved
IK.IK_Tripod_B(
"support"
) # !!! swing-nél a még a régi IK dict van használatban !!!
kematox.MoveTripodB("default", "swing", 1000) #750
walkval["tripod_step_1_complete"] = True
elif walkval["tripod_step_1_complete"] == True:
# 1. TRIPOD B support body and translates
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "support", 750)
# 2. TRIPOD A swigns -> raise TRIPOD A
z_saved = IK.IK_in["POS_Z"]
y_saved = IK.IK_in["POS_Y"]
IK.IK_in["POS_Z"] = defineStepHeight()
IK.IK_in["POS_X"] = 0.0 # new
IK.IK_in["POS_Y"] = 0.0
IK.IK_in["ROT_Z"] = 0.0
IK.IK_Tripod_A(
"support"
) # !!! swing-nél a még a régi IK dict van használatban !!!
kematox.MoveTripodA("default", "swing", 750)
# 3. TRIPOD A swings -> lowering TRIPOD A
IK.IK_in["POS_Z"] = z_saved
IK.IK_Tripod_A(
"support"
) # !!! swing-nél a még a régi IK dict van használatban !!!
kematox.MoveTripodA("default", "swing", 1000) #750
walkval["tripod_step_1_complete"] = False
def SetReadyPos(kematox, mode):
"""Default values for ready stance "D"=225, "z"=110"""
if mode == "set":
"""STEP 1 - Robor lift"""
IK.IK_in["z"] = 110.0
IK.IK_SixLeg()
kematox.MoveSixLeg(1000, "swing")
"""STEP 2 - TRIPOD A lift legs"""
IK.IK_in["POS_Z"] = -50.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 3 - TRIPOD A lower legs"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 4 - TRIPOD B lift legs"""
IK.IK_in["D"] = 275.0
IK.IK_in["POS_Z"] = -50.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 5 - TRIPOD B lower legs"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 6 - TRIPOD A lift legs again to release stress in servos"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 7 - TRIPOD A lower legs"""
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 8 - TRIPOD B lift legs again to release stress in servos"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 9 - TRIPOD B release legs"""
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
elif mode == "return":
"""STEP 1 - return to default "z" at no matter what "D" value"""
IK.IK_in["POS_Z"] = 0
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "swing")
"""STEP 2 - TRIPOD A lift legs"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 3 - TRIPOD A lower legs. "D" is now equals id default value"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 4 - TRIPOD B lift legs"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 5 - TRIPOD B lower legs"""
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
def MoveBlock(self):
# (1) Approach to Taget Block (10cm above)
print "MoveBlock Step(1): Approach to Taget Block (Approximately)"
StartPosition = self.GetEndPointPosition()
MiddlePosition = [
self.Desired_Position[0], self.Desired_Position[1] - 0.10,
self.Desired_Position[2] + 0.10
]
EndPosition = [
self.Block_Position[0], self.Block_Position[1],
self.Block_Position[2] + 0.10
]
Accuracy = 0.03 # Rough
IK.IK_MoveIt(self.MoveIt_left_arm, StartPosition, MiddlePosition,
EndPosition, Accuracy)
# (2) Approach to Target Block
print "MoveBlock Step(2): Approach to Target Block"
StartPosition = self.GetEndPointPosition()
MiddlePosition = False
EndPosition = [
self.Block_Position[0], self.Block_Position[1],
self.Block_Position[2]
]
Accuracy = 0.005 # Accurate
IK.IK_MoveIt(self.MoveIt_left_arm, StartPosition, MiddlePosition,
EndPosition, Accuracy)
# (3) Suction acutator (lift block)
print "MoveBlock Step(3): Sucker works (lift block)"
self.Gripper_control(self.Gripper_left, 'close')
# (4) Move Block to Tower
print "MoveBlock Step(4): Move BLocks to House (Approximately)"
StartPosition = self.GetEndPointPosition()
MiddlePosition = [
self.Block_Position[0], self.Block_Position[1],
self.Block_Position[2] + 0.10, self.Desired_Position[0],
self.Desired_Position[1] - 0.10, self.Desired_Position[2] + 0.10
]
EndPosition = [
self.Desired_Position[0], self.Desired_Position[1],
self.Desired_Position[2] + 0.10
]
#if self.Counting <= 2 :
# Accuracy = 0.02 # Accurate
#elif:
Accuracy = 0.03 # Rough
IK.IK_MoveIt(self.MoveIt_left_arm, StartPosition, MiddlePosition,
EndPosition, Accuracy)
# (5) Move Block to Desired Position
print "MoveBlock Step(5): Move Block to Desired Position"
StartPosition = self.GetEndPointPosition()
MiddlePosition = False
EndPosition = [
self.Desired_Position[0], self.Desired_Position[1],
self.Desired_Position[2]
]
Accuracy = 0.005 # Accurate
IK.IK_MoveIt(self.MoveIt_left_arm, StartPosition, MiddlePosition,
EndPosition, Accuracy)
# (6) Suction stops (drop block)
print "MoveBlock Step(6): Sucker stops (drop block)"
self.Gripper_control(self.Gripper_left, 'open')
# (7) Lift 5cm up to avoid collision with the built house
print "MoveBlock Step(7): Lift 10cm up to aviod collision with the built house"
StartPosition = self.GetEndPointPosition()
MiddlePosition = False # no middle point
EndPosition = [
self.Desired_Position[0], self.Desired_Position[1],
self.Desired_Position[2] + 0.05
]
Accuracy = 0.005 # Accurate
IK.IK_MoveIt(self.MoveIt_left_arm, StartPosition, MiddlePosition,
EndPosition, Accuracy)
trajoptpy.SetInteractive(args.interactive) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
collisionChecker = op.RaveCreateCollisionChecker(env,'pqp')
collisionChecker.SetCollisionOptions(op.CollisionOptions.Distance|op.CollisionOptions.Contacts)
env.SetCollisionChecker(collisionChecker)
joint_start1 = [3.14/3, 3.14/4, 0]
robot.SetDOFValues(joint_start1, robot.GetManipulator('left_arm').GetArmIndices())
joint_start2 = [-3.14/4, 3.14/4, 0]
robot.SetDOFValues(joint_start2, robot.GetManipulator('right_arm').GetArmIndices())
# manip = robot.SetActiveManipulator('right_arm')
time.sleep(0.1) # Give time for the environment to update
IK_obj = IK.dVRK_IK_simple() # Creates an IK object
endEff = IK_obj.get_endEffector_fromDOF([-3.14/2, 3.14/4, 0])
joint_target = IK_obj.get_joint_DOF(endEff)
manip = "right_arm"
startEff = IK_obj.get_endEffector_fromDOF(joint_start2)
print np.linalg.norm(np.array(endEff) - np.array(startEff))
# IPython.embed()
request = {
"basic_info" : {
"n_steps" : 200,
"manip" : manip, # see below for valid values
"start_fixed" : True # i.e., DOF values at first timestep are fixed based on current robot state
},
test_angle = [pi / 4, pi / 4, pi / 4, pi / 4,0]
FK_result = FK.fk_srv(test_angle)
print(FK_result)
print("above is FK")
## VK service
jac = VK.vk_srv(current_angles)
print(jac)
print("above is VK")
## IK service
#goal_position = [FK_result[0, 3], FK_result[1, 3], FK_result[2, 3]]
#goal_position = [0.175, 0, 0.08]
rotating_angle = list(IK.ik_srv(goal_position))
rotating_angle.append(servo_ID1)
#rotating_angle[4] = -(rotating_angle[1] + rotating_angle[2] + rotating_angle[3]) + (pi / 2)
rotating_angle[4] = 0
print("rotating angle:", rotating_angle)
for i in range(0, 3):
servo_degree[i] = int(-(((rotating_angle[i] * 360) / (2 * pi)) / 0.24) - 0.5)
servo_degree[3] = int((((rotating_angle[3] * 360) / (2 * pi)) / 0.24) + 0.5)
servo_degree[4] = int((((rotating_angle[4] * 360) / (2 * pi)) / 0.24) + 0.5)
servo_degree[5] = int((((rotating_angle[5] * 360) / (2 * pi)) / 0.24) + 0.5)
print("servo degree:", servo_degree)
## serial communication
import unittest
import IK
from constants import const
ik = IK()
class TestAudio(unittest.TestCase):
def test_audio_process_Command(self):
self.assertTrue(isinstance(orden, int), "Should be of type int")
self.assertEqual(ik, (None, None), "Argument should be and number")
if __name__ == '__main__':
unittest.main()
def setT(self, omega, phi, psi, xm, ym, zm):
(self.Tlf, self.Trf, self.Tlb,
self.Trb) = IK.bodyIK(omega, phi, psi, xm, ym, zm)
def EventExecute(event, mode_dict, flag_dict, auxval, walkval):
if mode_dict["mode"] == 0: # IDLE
if flag_dict["position_reached"] == False:
if flag_dict["return_to_Idle"] == True:
print "MAIN: Returning to IDLE"
funct.SetIdlePos(kematox, "return")
print "MAIN: IDLE position reached\n"
flag_dict["return_to_Idle"] = False
flag_dict["position_reached"] = True
elif flag_dict["return_to_Idle"] == False:
print "MAIN: MODE set to IDLE" # ide még a fejet idle-be parancsot be kell szúrni
funct.SetIdlePos(kematox, "set")
print "MAIN: IDLE position reached\n"
flag_dict["position_reached"] = True
elif flag_dict["position_reached"] == True:
pass
elif mode_dict["mode"] == 1: # READY
if flag_dict["position_reached"] == False:
if flag_dict["return_to_Ready"] == True:
print "MAIN: Returning to READY"
funct.SetReadyPos(kematox, "return")
funct.CenterHead(kematox)
print "MAIN: READY position reached\n"
flag_dict["return_to_Ready"] = False
flag_dict["position_reached"] = True
elif flag_dict["return_to_Ready"] == False:
print "MAIN: MODE set to READY"
funct.SetReadyPos(kematox, "set")
funct.CenterHead(kematox)
print "MAIN: READY position reached\n"
flag_dict["position_reached"] = True
elif flag_dict["position_reached"] == True:
pass
elif mode_dict["mode"] == 2: # STATIC
if flag_dict["flag_headModeSelected"] == False:
if flag_dict["position_reached"] == False:
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "support")
#IK.IK_Diag(IK.IK_out)
flag_dict["position_reached"] = True
print "DIAG: MOVEMENT READY"
elif flag_dict["position_reached"] == True:
pass
elif flag_dict["flag_headModeSelected"] == True:
if flag_dict["position_reached"] == False:
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "support")
IK.CalcHeadPos(HeadMovInput, HeadCalibrVal, HeadMovOutput)
kematox.MoveHead(HeadMovOutput, 500)
#IK.IK_Diag(IK.IK_out)
flag_dict["position_reached"] = True
print "DIAG: MOVEMENT READY"
elif flag_dict["position_reached"] == True:
pass
elif mode_dict["mode"] == 3: # WALK
WalkVector = IK.CalcWalkVector()
#if ((abs(IK.IK_in["ROT_Z"]) > 0 and IK.IK_in["POS_Z"]) or (IK.IK_in["ROT_Y"] > 0 and IK.IK_in["POS_Z"] > 0) or (WalkVector > 0 and IK.IK_in["POS_Z"] > 0) or (WalkVector > 0 and IK.IK_in["ROT_Y"] > 0 and IK.IK_in["POS_Z"] > 0)):
if ((abs(IK.IK_in["ROT_Z"]) > 0 and IK.IK_in["POS_Z"])
or (WalkVector > 0 and IK.IK_in["POS_Z"] > 0)
or (WalkVector > 0 and IK.IK_in["ROT_Y"] > 0
and IK.IK_in["POS_Z"] > 0)):
print "WALK"
funct.TripodWalk(kematox, walkVal)
elif (IK.IK_in["POS_Z"] > 0 or IK.IK_in["ROT_Y"] > 0 or WalkVector > 0
or (IK.IK_in["ROT_Y"] > 0 and WalkVector > 0)
or (IK.IK_in["POS_Z"] == 0 and IK.IK_in["ROT_Y"] == 0
and WalkVector == 0)):
"""Akkor is STATIC legyen a mód, ha minden 0 -> visszatérjen a robot alaphelyzetbe """
print "STATIC"
if flag_dict["position_reached"] == False:
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "support")
flag_dict["position_reached"] = True
print "DIAG: MOVEMENT READY"
elif flag_dict["position_reached"] == True:
pass
def main():
#LH
link1EndX = link1startX - getL1End(l1)[0] # also link2startX
link1EndY = getL1End(l1)[1] # also link2startY
link2EndX = position.defaultAngle(True, l2, link1EndX, link1EndY)[0]
link2EndY = position.defaultAngle(True, l2, link1EndX, link1EndY)[1]
#RH
link1EndXrh = link1startXrh + getL1End(l1)[0] # also link2startX
link1EndYrh = getL1End(l1)[1] # also link2startY
link2EndXrh = position.defaultAngle(False, l2, link1EndXrh, link1EndYrh)[0]
link2EndYrh = position.defaultAngle(False, l2, link1EndXrh, link1EndYrh)[1]
plt.axis('equal')
# plot link1
plt.plot([link1startX, link1EndX], [link1startY, link1EndY], 'bo-')
# plot link2
plt.plot([link1EndX, link2EndX], [link1EndY, link2EndY], 'bo-')
# plot hat
hatCoord = position.hatCoordinate(hatLength, link2EndX, link2EndY)
plt.plot([link2EndX, hatCoord[0]], [link2EndY, hatCoord[1]], 'r-')
#plot hat2 - Start of link2
hatCoord2 = position.hatCoordinate(hatLength, link1EndX, link1EndY)
plt.plot([link1EndX, hatCoord2[0]], [link1EndY, hatCoord2[1]], 'r-')
# plot link1 -RH
plt.plot([link1startXrh, link1EndXrh], [link1startYrh, link1EndYrh], 'bo-')
# plot link2 -RH
plt.plot([link1EndXrh, link2EndXrh], [link1EndYrh, link2EndYrh], 'bo-')
# plot hat - RH
hatCoord = position.hatCoordinateRight(hatLength, link2EndXrh, link2EndYrh)
plt.plot([link2EndXrh, hatCoord[0]], [link2EndYrh, hatCoord[1]], 'r-')
# plot hat2 - RH
hatCoord2 = position.hatCoordinateRight(hatLength, link1EndXrh,
link1EndYrh)
plt.plot([link1EndXrh, hatCoord2[0]], [link1EndYrh, hatCoord2[1]], 'r-')
global beamOriginX
global beamOriginY
beamOriginX = randint(int(link1EndX) - 500, int(link1startX) - 100)
beamOriginY = randint(int(link1EndY) + 850, int(link1EndY) + 1000)
# plot beam horizontal member
plt.plot([beamOriginX, beamOriginX + beamLength],
[beamOriginY, beamOriginY], 'g-')
# plot beam vertical member
plt.plot([beamOriginX, beamOriginX],
[beamOriginY, beamOriginY + beamLength], 'g-')
# beam top left corner coordinates
goalX = beamOriginX
goalY = beamOriginY + beamLength
# beam bottom right corner coordinates
goalXbottom = beamOriginX + beamLength
goalYbottom = beamOriginY
global updatedLink1length
global updatedLink2length
global updatedLink1lengthRH
global updatedLink2lengthRH
# solve for the end coordinates
updatedLink1length = abs(IK.solveL11(goalX))
updatedLink2length = abs(IK.solveL22(goalX, goalY))
updatedLink1lengthRH = abs(IK.solveL11(goalXbottom))
# Hardcoded
# updatedLink2lengthRH = abs(IK.solveL222(goalXbottom, goalYbottom))
link1EndYrh = getL1End(updatedLink1lengthRH)[1] # also link2startY
updatedLink2lengthRH = goalYbottom - link1EndYrh
axnext = plt.axes([0.81, 0.05, 0.1, 0.075])
bnext = Button(axnext, 'step1')
bnext.on_clicked(next)
plt.show()
end = np.array(waypoints[i + 1])
lst += [
list(start + ii * (end - start) / (n)) for ii in range(n + 1)
]
return ik.get_joint_DOF(lst)
if __name__ == "__main__":
joint_start1 = [3.14 / 3, 3.14 / 4, 2]
joint_start2 = [-3.14 / 5, 3.14 / 4, 0]
manip = "right_arm"
planner = Motion_planning('env.xml', "right_arm")
planner.init_collision_checker('pqp', [op.CollisionOptions.Contacts])
planner.set_manip(name="left_arm", DOF=joint_start1)
planner.set_manip(name="right_arm", DOF=joint_start2)
IK_obj = IK.dVRK_IK_simple() # Creates an IK object
endEff = IK_obj.get_endEffector_fromDOF([-3.14 / 2, 3.14 / 4, -8])
joint_target = IK_obj.get_joint_DOF(endEff)
planner.optimize(manip, joint_target, algorithm="RRTConnect")
planner.simulate()
planner.optimize(manip, [joint_start2], algorithm="RRTConnect")
planner.simulate()
IPython.embed()
# coding: UTF-8
import IK, util, numpy
from PIL import Image
editor = IK.IKimage()
dic_config = util.lerConfig()
def getMemeConfig(nome):
return dic_config.get(nome)
def adicionaImagem(imagem_meme, imagem_nova, posis):
matriz_meme = editor.asArray(imagem_meme)
matriz_imagem = editor.asArray(imagem_nova)
pos1, pos2 = posis
print len(matriz_meme), len(matriz_meme[0])
pos1 = int((float(pos1) / 100) * len(matriz_meme))
pos2 = int((float(pos2) / 100) * len(matriz_meme[0]))
for i in range(len(matriz_imagem)):
for j in range(len(matriz_imagem[i])):
if len(matriz_meme) < i + pos1 + 1 or len(
matriz_meme[0]) < j + pos2 + 1:
continue
matriz_meme[i + pos1][j + pos2] = matriz_imagem[i][j]
imagem = Image.fromarray(matriz_meme)
return imagem
def main():