placo

import placo import numpy as np # Create a robot wrapper from an online URDF (e.g., UR5 robot) # This avoids local file dependencies for a quick test. # Ensure internet connectivity for the URDF download. robot = placo.RobotWrapper( "https://raw.githubusercontent.com/ros-industrial/universal_robot/kinetic-devel/ur_description/urdf/ur5_robot.urdf", placo.Flags.FLOATING_BASE # Set to true if the robot base is free-floating (e.g., a humanoid) ) # Set an initial configuration for the robot's joints robot.set_joint_positions({ "shoulder_pan_joint": 0.0, "shoulder_lift_joint": -np.pi / 2, "elbow_joint": np.pi / 2, "wrist_1_joint": -np.pi / 2, "wrist_2_joint": -np.pi / 2, "wrist_3_joint": 0.0 }) robot.update_kinematics() # Update the robot's internal kinematics based on current joint positions # Get the current transform (position and orientation) of the 'tool0' end-effector T_tool0_world = robot.get_transform("tool0") current_position = T_tool0_world.translation current_orientation = T_tool0_world.rotation # as a 3x3 rotation matrix # Define a target for the end-effector: move it slightly forward and up target_position = current_position + np.array([0.1, 0.0, 0.05]) # Move 10cm in X, 5cm in Z target_orientation = current_orientation # Keep the current orientation # Create a kinematics solver instance solver = placo.KinematicsSolver(robot) # Add tasks to the solver # PositionTask: Make the 'tool0' frame reach the target_position position_task = placo.PositionTask(robot.frame_id("tool0"), target_position) position_task.set_weight(1.0) # Set task weight (importance) solver.add_task(position_task) # OrientationTask: Make the 'tool0' frame match the target_orientation orientation_task = placo.OrientationTask(robot.frame_id("tool0"), target_orientation) orientation_task.set_weight(0.5) # Orientation is less critical than position solver.add_task(orientation_task) # Solve the inverse kinematics problem solver.solve(verbose=False) # Set verbose=True for detailed solver output robot.update_kinematics() # Apply the newly calculated joint positions to the robot model # Print the results new_T_tool0_world = robot.get_transform("tool0") print(f"Initial tool0 position: {current_position.round(3)}") print(f"Target tool0 position: {target_position.round(3)}") print(f"Final tool0 position: {new_T_tool0_world.translation.round(3)}")